runscript
Generating reference trajectory.
Tsc_initial?{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('runscript')" style="font-weight:bold">runscript</a>
} 
runscript
{输入参数的数目不足。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('runscript', '/home/zzh/MATLAB Add-Ons/Toolboxes/Robotics Toolbox for MATLAB/lib/common/runscript.m', 84)" style="font-weight:bold">runscript</a> (<a href="matlab: opentoline('/home/zzh/MATLAB Add-Ons/Toolboxes/Robotics Toolbox for MATLAB/lib/common/runscript.m',84,0)">第 84 行</a>)
        fname = [fname '.m'];
} 
load("robotic_arm.mat");
opentoline('/home/zzh/MATLAB Add-Ons/Toolboxes/Robotics Toolbox for MATLAB/lib/common/runscript.m',84,0)
main_matlab
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('vertcat')" style="font-weight:bold">vertcat</a>
要串联的数组的维度不一致。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('compute_Slist_Blist', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m', 42)" style="font-weight:bold">compute_Slist_Blist</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m',42,0)">第 42 行</a>)
            Adjoint_T = [R_relative, zeros(3, 3); adjoint_lower];

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('robotic_arm_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m', 23)" style="font-weight:bold">robotic_arm_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m',23,0)">第 23 行</a>)
    [arm.Slist,arm.Blist]=compute_Slist_Blist(arm.joint_base);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m', 8)" style="font-weight:bold">mobile_robot_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',8,0)">第 8 行</a>)
    mobile_robot.arm = robotic_arm_model(w, M);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 11)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',11,0)">第 11 行</a>)
mobile_robot1 = mobile_robot_model();
} 
main_matlab
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('vertcat')" style="font-weight:bold">vertcat</a>
要串联的数组的维度不一致。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('compute_Slist_Blist', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m', 42)" style="font-weight:bold">compute_Slist_Blist</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m',42,0)">第 42 行</a>)
            Adjoint_T = [R_relative, zeros(3, 3); adjoint_lower];

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('robotic_arm_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m', 23)" style="font-weight:bold">robotic_arm_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m',23,0)">第 23 行</a>)
    [arm.Slist,arm.Blist]=compute_Slist_Blist(arm.joint_base);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m', 8)" style="font-weight:bold">mobile_robot_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',8,0)">第 8 行</a>)
    mobile_robot.arm = robotic_arm_model(w, M);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 11)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',11,0)">第 11 行</a>)
mobile_robot1 = mobile_robot_model();
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',8,0)
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m',23,0)
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m',42,0)
main_matlab
length(Tlist)

ans =

     7

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('vertcat')" style="font-weight:bold">vertcat</a>
要串联的数组的维度不一致。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('compute_Slist_Blist', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m', 41)" style="font-weight:bold">compute_Slist_Blist</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m',41,0)">第 41 行</a>)
            Adjoint_T = [R_relative, zeros(3, 3); adjoint_lower];

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('robotic_arm_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m', 23)" style="font-weight:bold">robotic_arm_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m',23,0)">第 23 行</a>)
    [arm.Slist,arm.Blist]=compute_Slist_Blist(arm.w,arm.joint_base);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m', 8)" style="font-weight:bold">mobile_robot_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',8,0)">第 8 行</a>)
    mobile_robot.arm = robotic_arm_model(w, M);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 11)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',11,0)">第 11 行</a>)
mobile_robot1 = mobile_robot_model();
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/compute_Slist_Blist.m',41,0)
main_matlab
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mtimes')" style="font-weight:bold"> * </a>
用于矩阵乘法的维度不正确。请检查并确保第一个矩阵中的列数与第二个矩阵中的行数匹配。要单独对矩阵的每个元素进行运算，请使用 TIMES (.*)执行按元素相
乘。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('JacobianSpace', '/home/zzh/下载/robot/ModernRobotics-master/packages/MATLAB/mr/JacobianSpace.m', 31)" style="font-weight:bold">JacobianSpace</a> (<a href="matlab: opentoline('/home/zzh/下载/robot/ModernRobotics-master/packages/MATLAB/mr/JacobianSpace.m',31,0)">第 31 行</a>)
	Js(:, i) = Adjoint(T) * Slist(:, i);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('robotic_arm_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m', 47)" style="font-weight:bold">robotic_arm_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m',47,0)">第 47 行</a>)
    arm.J_space = JacobianSpace(arm.Slist,arm.q);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m', 8)" style="font-weight:bold">mobile_robot_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',8,0)">第 8 行</a>)
    mobile_robot.arm = robotic_arm_model(w, M);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 11)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',11,0)">第 11 行</a>)
mobile_robot1 = mobile_robot_model();

<a href="matlab:helpview('matlab','error_innerdim')" style="font-weight:bold">相关文档</a>
} 
opentoline('/home/zzh/下载/robot/ModernRobotics-master/packages/MATLAB/mr/JacobianSpace.m',31,0)
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/robotic_arm_model.m',47,0)
main_matlab
{无法执行赋值，因为左侧的大小为 1×7，右侧的大小为 6×1。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('update_robotic_arm', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/update_robotic_arm.m', 20)" style="font-weight:bold">update_robotic_arm</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/update_robotic_arm.m',20,0)">第 20 行</a>)
        arm.Slist(i,:) = ScrewToAxis (p, arm.w{i}, 0);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m', 9)" style="font-weight:bold">mobile_robot_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',9,0)">第 9 行</a>)
    update_robotic_arm(mobile_robot.arm,mobile_robot.car,zeros(7,1),0.25)

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 11)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',11,0)">第 11 行</a>)
mobile_robot1 = mobile_robot_model();
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/robotic_arm/update_robotic_arm.m',20,0)
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',73,1)">73  </a>    dx = fmincon(fun1, X, [], [], [], [], [], [], fun2, options);
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 Joint Angles: [-0.000121114227682437;0.185441233024398;-0.000121114227682768;1.86264489701995e-09;-0.000121114227682768;-0.225185848658087;-0.000121114227682768]
Mobile Platform 2 Pose: [-2.35619437191491;9.99866771753545;9.81844390937036]
Robot Arm 2 Joint Angles: [0.000121114227682424;-0.185441233024398;0.000121114227682774;-1.86264487453752e-09;0.000121114227682774;0.213165194053599;0.000121114227682774]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814488;-9.99718189953243;-9.65601765681]
Robot Arm 1 Joint Angles: [-0.000242257227146163;0.370882466048795;-0.000242257227146826;-0.00011813576366936;-0.000242257227146826;-0.450777934956315;-0.000242257227146826]
Mobile Platform 2 Pose: [-2.35619425360615;9.99733167719312;9.63690106041641]
Robot Arm 2 Joint Angles: [0.000242260535616737;-0.370882466048795;0.000242260535617437;8.24406669025269e-05;0.000242260535617437;0.426849204164602;0.000242260535617437]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 Joint Angles: [-0.000363286279800703;0.556323699073193;-0.000363286279801698;-0.000357282809229025;-0.000363286279801698;-0.676844213293331;-0.000363286279801698]
Mobile Platform 2 Pose: [-2.35619413529215;9.99599115574577;9.45545454589275]
Robot Arm 2 Joint Angles: [0.000363412210349703;-0.556323699073193;0.000363412210350752;0.000250017184069795;0.000363412210350752;0.641165057095127;0.000363412210350752]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 Joint Angles: [-0.000483866411479533;0.74176493209759;-0.000483866411480859;-0.00071951007623215;-0.000483866411480859;-0.903917115513837;-0.000483866411480859]
Mobile Platform 2 Pose: [-2.35625815749602;9.99464273095702;9.27451030581246]
Robot Arm 2 Joint Angles: [0.000484371855473402;-0.74176493209759;0.000484371855474824;0.000505156626171679;0.000484371855474824;0.856657882162596;0.000484371855474824]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 Joint Angles: [-0.000603674592608101;0.927206165121988;-0.000603674592609757;-0.00120731922081315;-0.000603674592609757;-1.13251733087994;-0.000603674592609757]
Mobile Platform 2 Pose: [-2.35631718385582;9.99328288466748;9.0945122115477]
Robot Arm 2 Joint Angles: [0.000604961580060246;-0.927206165121988;0.000604961580062008;0.000851202888537353;0.000604961580062008;1.07388731694669;0.000604961580062008]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346624]
Robot Arm 1 Joint Angles: [-0.000722796083224461;1.11264739814639;-0.000722796083226446;-0.00182553790636017;-0.000722796083226446;-1.36238137284088;-0.000722796083226446]
Mobile Platform 2 Pose: [-2.35637444284136;9.99191235634025;8.91540971396643]
Robot Arm 2 Joint Angles: [0.000725372745482353;-1.11264739814639;0.000725372745484459;0.00129364618325321;0.000725372745484459;1.2927294777557;0.000725372745484459]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528708;-9.99002831581144;-8.79949883355178]
Robot Arm 1 Joint Angles: [-0.00084158055635903;1.29808863117078;-0.000841580556361343;-0.00257974514714835;-0.000841580556361343;-1.59288971083287;-0.000841580556361343]
Mobile Platform 2 Pose: [-2.35643666636944;9.99053428065779;8.73682110701411]
Robot Arm 2 Joint Angles: [0.000846049543728647;-1.29808863117078;0.000846049543731091;0.00183860862163513;0.000846049543731091;1.51267651635453;0.000846049543731091]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274174]
Robot Arm 1 Joint Angles: [-0.00095982998704643;1.48352986419518;-0.00095982998704907;-0.00347025803777757;-0.00095982998704907;-1.82450740028498;-0.00095982998704907]
Mobile Platform 2 Pose: [-2.35643654824403;9.98914654660056;8.55900555539277]
Robot Arm 2 Joint Angles: [0.00096700810854181;-1.48352986419518;0.000967008108544598;0.00248884854374086;0.000967008108544598;1.73406372351854;0.000967008108544598]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196128;-9.98711511781;-8.46062525935788]
Robot Arm 1 Joint Angles: [-0.00107743739450421;1.66897109721958;-0.00107743739450718;-0.00449854422257735;-0.00107743739450718;-2.05708258800973;-0.00107743739450718]
Mobile Platform 2 Pose: [-2.35643642975467;9.98774923983355;8.38210530725081]
Robot Arm 2 Joint Angles: [0.00108833935090189;-1.66897109721958;0.00108833935090501;0.00324801541781589;0.00108833935090501;1.95687363730637;0.00108833935090501]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929674;-9.98565931998689;-8.29072857539469]
Robot Arm 1 Joint Angles: [-0.00119546838173931;1.85441233024398;-0.00119546838174261;-0.00567665166884604;-0.00119546838174261;-2.0943951023932;-0.00119546838174261]
Mobile Platform 2 Pose: [-2.35643630997691;9.98635048842027;8.20503302700127]
Robot Arm 2 Joint Angles: [0.00121098991064428;-1.85441233024398;0.00121098991064088;0.0041254592633564;0.00121098991064775;2.0943951023932;0.00121098991064775]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.98418232823806;-8.12462828169963]
Robot Arm 1 Joint Angles: [-0.00131696062399761;2.03985356326837;-0.00131696062400122;-0.00639922022289152;-0.00131696062400122;-2.30399143685392;-0.00131696052349854]
Mobile Platform 2 Pose: [-2.35620613465631;9.9849370025393;8.03016547440579]
Robot Arm 2 Joint Angles: [0.00133410549808167;-2.03985356326837;0.00133410549807795;0.00485302507233282;0.00133410549808566;2.2968130786795;0.00133410541656935]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840343631;-7.95994828885485]
Robot Arm 1 Joint Angles: [-0.00143870881546709;2.14949927567335;-0.00143870881547102;-0.00719336612686439;-0.00143870881547102;-2.22826414942432;-0.00143870871496834]
Mobile Platform 2 Pose: [-2.35594970124224;9.9835183946618;7.85622695494204]
Robot Arm 2 Joint Angles: [0.00145825418979129;-2.14094730880621;0.00145825418978549;0.00563275427424487;0.00145825418979575;2.38506375019459;0.00145825488097536]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455233642;-7.79314279534789]
Robot Arm 1 Joint Angles: [-0.00156630058254284;2.18077941225183;-0.00156630058254709;-0.00742071387543599;-0.00156630058254709;-2.20427246494567;-0.00156630048204441]
Mobile Platform 2 Pose: [-2.35503667148254;9.98210777301549;7.68071204608006]
Robot Arm 2 Joint Angles: [0.0015851290770819;-2.17629592312991;0.00158512907707163;0.00638509484061451;0.00158512907708669;2.2490794504243;0.0015851297682663]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.97972356094619;-7.63794018920194]
Robot Arm 1 Joint Angles: [-0.00168692887755489;2.19333238381216;-0.00168692887755944;-0.00746871707029978;-0.00168692887755944;-2.20035149692109;-0.00168692877705676]
Mobile Platform 2 Pose: [-2.35501435791909;9.98065392763296;7.51407473428465]
Robot Arm 2 Joint Angles: [0.00170724332436555;-2.18881101472958;0.0017072433243508;0.00658732532989573;0.00170724332437067;2.21052473384201;0.00170724401555028]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591664647;-7.46636918682404]
Robot Arm 1 Joint Angles: [-0.00182021590843541;2.1916426546686;-0.00182021590844029;-0.0074668554106574;-0.00182021590844029;-2.19374950637937;-0.00182021580793761]
Mobile Platform 2 Pose: [-2.35553939160444;9.97928556134656;7.33680817218772]
Robot Arm 2 Joint Angles: [0.00183847520683103;-2.18841630056477;0.0018384752068118;0.00663681749037881;0.00183847520683648;2.1949007202199;0.00183847589801609]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346213;-9.97678818483778;-7.31817266006872]
Robot Arm 1 Joint Angles: [-0.00193743348788856;2.19689567720104;-0.00193743348789372;-0.00745079646704906;-0.00193743348789372;-2.19753736886275;-0.00193743338739104]
Mobile Platform 2 Pose: [-2.35719380965723;9.97781162309103;7.17683116728257]
Robot Arm 2 Joint Angles: [0.00195756969294632;-2.19335033688794;0.00195756969292259;0.00663624888132464;0.00195756969295208;2.19529600473222;0.00195757038413169]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232726;-9.9753590119527;-7.16418511940773]
Robot Arm 1 Joint Angles: [-0.00205940439551018;2.19373921816966;-0.00205940439551565;-0.00742961274493574;-0.00205940439551565;-2.19394443668375;-0.00205940429501297]
Mobile Platform 2 Pose: [-2.35748279680039;9.97642670530195;7.00854875336465]
Robot Arm 2 Joint Angles: [0.00208410411746768;-2.28770643892318;0.00208410262327842;0.00662124465443168;0.00208410262331057;2.2895659042854;0.00208410331449018]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785396026760021;-9.9743099077894;-7.00084677753696]
Robot Arm 1 Joint Angles: [-0.00218788714244672;2.17454821889181;-0.0021878871424525;-0.00740588626491103;-0.0021878871424525;-2.17462402111636;-0.00218788704194981]
Mobile Platform 2 Pose: [-2.35922187096734;9.97519906456786;6.84165468292919]
Robot Arm 2 Joint Angles: [0.00221142451529212;-2.21681974611415;0.00221142302109836;0.00660597836957832;0.00221142302113533;2.21187767382213;0.00221142371231493]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785395956603331;-9.97259808477927;-6.91581468782267]
Robot Arm 1 Joint Angles: [-0.00225972628881696;2.20060448107323;-0.00225972628882291;-0.00739236729819143;-0.00225972628882291;-2.20063532500117;-0.00225972618832022]
Mobile Platform 2 Pose: [-2.36092971612974;9.97359603815626;6.72789466167286]
Robot Arm 2 Joint Angles: [0.00229767215388615;-2.20806025355593;0.00229767065969262;0.00658087281146497;0.00229767065972958;2.20659266856525;0.00229767135090919]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227854;-9.97409553094492;-6.77330520536714]
Robot Arm 1 Joint Angles: [-0.00236353291585721;2.01516324804883;-0.00236353291586342;-0.00737275757568851;-0.00236353291586342;-1.96106355016073;-0.00236353281536074]
Mobile Platform 2 Pose: [-2.36092957741403;9.97448159350473;6.54709323299883]
Robot Arm 2 Joint Angles: [0.00243971704139683;-2.06634248535816;0.00243971554720365;0.00655196036982462;0.00243971554724061;2.06493272525077;0.00243971623842022]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.9722812970083;-6.73833367380644]
Robot Arm 1 Joint Angles: [-0.00239739691375578;2.0943950974136;-0.00239739691376206;-0.00731934997979104;-0.00239739691376206;-2.09439511080696;-0.00239739681325938]
Mobile Platform 2 Pose: [-2.36235905247552;9.97261212672092;6.4893993258835]
Robot Arm 2 Joint Angles: [0.00248224510409184;-2.17571102969911;0.00248224360989877;0.00654327735525158;0.00248224360993573;2.17529331619379;0.00248224430111534]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540073;-9.97333331320683;-6.6973716673834]
Robot Arm 1 Joint Angles: [-0.00242465134050779;1.92606731995396;-0.00242465134051415;-0.00731416428434603;-0.00242465134051415;-1.92607770473821;-0.00242465124001147]
Mobile Platform 2 Pose: [-2.3623590143338;9.97397067280216;6.44207557475005]
Robot Arm 2 Joint Angles: [0.00252130036727536;-1.99026979667471;0.00252129887308238;0.00653635981088386;0.00252129887311934;1.95793286284526;0.00252129956429895]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.78539578119585;-9.97294909647023;-6.68023583492612]
Robot Arm 1 Joint Angles: [-0.00243933982504677;1.98753916232563;-0.00243933982505317;-0.00731132879957033;-0.00243933982505317;-1.98755521807943;-0.00243933972455049]
Mobile Platform 2 Pose: [-2.36235899592969;9.97313054469888;6.41628062585664]
Robot Arm 2 Joint Angles: [0.00254014431418212;-2.0943951023932;0.00254014281998918;0.00651752707777928;0.00254014282002614;2.09237219307912;0.00254014351120575]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.786583728921051;-9.97200303867874;-6.67348002163193]
Robot Arm 1 Joint Angles: [-0.00244810023692547;2.0943951023932;-0.00244810023693188;-0.00730965013647538;-0.00244810023693188;-2.0943951023932;-0.002448100136436]
Mobile Platform 2 Pose: [-2.36235898472469;9.97273042038539;6.4011972075873]
Robot Arm 2 Joint Angles: [0.00255161636804164;-2.0943951023932;0.00255161487384873;0.00651501856631029;0.0025516148738857;2.0943951023932;0.0025516155650653]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.787820215400374;-9.97358488636938;-6.658489888061]
Robot Arm 1 Joint Angles: [-0.00245346351203854;1.9089538693049;-0.00245346351204498;-0.00730863145473396;-0.00245346351204498;-1.84130049432083;-0.00245346341154909]
Mobile Platform 2 Pose: [-2.36235897802882;9.97385924458085;6.39412700240987]
Robot Arm 2 Joint Angles: [0.00255847293778311;-1.91378208418802;0.00255847144359022;0.00651387163539225;0.00255847144362718;1.91378437804986;0.00255847213480679]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.789055691010501;-9.97165007568431;-6.66429527529198]
Robot Arm 1 Joint Angles: [-0.00245662817237413;2.09439510199037;-0.00245662817238056;-0.00730247937466522;-0.00245662817238056;-2.09439510096607;-0.00245662807189154]
Mobile Platform 2 Pose: [-2.36235897418051;9.9720090104827;6.38613180311934]
Robot Arm 2 Joint Angles: [0.002562411752238;-2.09439510008311;0.00256241025804512;0.00651320359973634;0.00256241025808208;2.09439510059301;0.00256241094926169]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.790290227280904;-9.97334587554025;-6.65299765973811]
Robot Arm 1 Joint Angles: [-0.00245854444204544;1.90895386897707;-0.00245854444205188;-0.00730211693698377;-0.00245854444205188;-1.82306749033738;-0.00245854434156287]
Mobile Platform 2 Pose: [-2.3623589718368;9.97390385696592;6.38615432268034]
Robot Arm 2 Joint Angles: [0.00256480984253003;-1.90895386636861;0.00256480834833715;0.00651280402683668;0.00256480834837412;1.79987078931938;0.00256480903955372]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.79029022613362;-9.97254300149928;-6.65580170335281]
Robot Arm 1 Joint Angles: [-0.00245971926100261;2.03741087873113;-0.00245971926100905;-0.00729928013613038;-0.00245971926100905;-1.95153017369315;-0.00245971916052003]
Mobile Platform 2 Pose: [-2.36235897039731;9.97264986912869;6.38204122085557]
Robot Arm 2 Joint Angles: [0.00256628388492966;-2.10958750964745;0.00256628239073679;0.00650839334914099;0.00256628239077375;2.00051325395361;0.00256628308195336]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.790290225430749;-9.97237811509803;-6.65550620131425]
Robot Arm 1 Joint Angles: [-0.00246043900043451;2.0637909846094;-0.00246043900044096;-0.00729756181311732;-0.00246043900044096;-1.97791371621745;-0.00246043889995194]
Mobile Platform 2 Pose: [-2.36235896951956;9.97310603046311;6.38182860558393]
Robot Arm 2 Joint Angles: [0.00256718270316793;-2.03659905461474;0.00256718120897507;0.00650575182435547;0.00256718120901203;1.92753008197047;0.00256718190019164]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.790290225008633;-9.97341831887358;-6.64975958644216]
Robot Arm 1 Joint Angles: [-0.00246087124782081;1.89735735089855;-0.00246087124782726;-0.00729653219115224;-0.00246087124782726;-1.81148214175053;-0.00246087114733825]
Mobile Platform 2 Pose: [-2.36235896898918;9.9735771498413;6.3816721400189]
Robot Arm 2 Joint Angles: [0.0025677258069506;-1.96121834800374;0.00256772431275773;0.00650414572388438;0.00256772431279469;1.85215258756041;0.0025677250039743]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.790290224744966;-9.97165700515629;-6.65837224068759]
Robot Arm 1 Joint Angles: [-0.00246114124222841;2.1791668931075;-0.00246114124223485;-0.00729587963387773;-0.00246114124223485;-2.09329298907402;-0.00246114114174583]
Mobile Platform 2 Pose: [-2.36235896866025;9.97221509864297;6.37922665370672]
Robot Arm 2 Joint Angles: [0.00256806263447364;-2.17914553690244;0.00256806114028078;0.00650314288926904;0.00256806114031774;2.07008178212835;0.00256806183149735]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.790290224588671;-9.97359339868868;-6.64819574401859]
Robot Arm 1 Joint Angles: [-0.00246130128871133;1.86934355024118;-0.00246130128871779;-0.00729550194974299;-0.00246130128871779;-1.78347040157598;-0.00246130118822877]
Mobile Platform 2 Pose: [-2.36235896846489;9.97401956819947;6.38179611249918]
Robot Arm 2 Joint Angles: [0.00256826267904104;-1.89042982336212;0.00256826118484818;0.0065025583883283;0.00256826118488514;1.7813672375899;0.00256826187606475]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.790290224490988;-9.97198118838939;-6.65650232316299]
Robot Arm 1 Joint Angles: [-0.00246140131582798;2.12729695575422;-0.00246140131583442;-0.00729525957579627;-0.00246140131583442;-2.04142429183691;-0.0024614012153454]
Mobile Platform 2 Pose: [-2.36235896834263;9.97211728518185;6.37853538630362]
Robot Arm 2 Joint Angles: [0.0025683878729218;-2.19479473098134;0.00256838637872894;0.00650218318900195;0.0025683863787659;2.08573289560778;0.00256838706994551]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.790290224432368;-9.97295374695729;-6.65125389844403]
Robot Arm 1 Joint Angles: [-0.00246146134284901;1.97168744265659;-0.00246146134285546;-0.00729511734192152;-0.00246146134285546;-1.88581506320703;-0.00246146124236644]
Mobile Platform 2 Pose: [-2.36235896826988;9.97371181273788;6.38110321060665]
Robot Arm 2 Joint Angles: [0.00256846237385183;-1.93967010461061;0.00256846087965897;0.00650196578315604;0.00256846087969593;1.83060870404874;0.00256846157087554]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.790290224396625;-9.97296922297084;-6.651349811845]
Robot Arm 1 Joint Angles: [-0.00246149794317022;1.96921119261804;-0.00246149794317667;-0.00729502947122296;-0.00246149794317667;-1.88333898890987;-0.00246149784268765]
Mobile Platform 2 Pose: [-2.36235896822501;9.97290941793671;6.37953315104436]
Robot Arm 2 Joint Angles: [0.00256850831743543;-2.06805313574356;0.00256850682324257;0.00650182872865292;0.00256850682327953;1.9589920092907;0.00256850751445914]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.790290224374528;-9.97196650277039;-6.65629133915637]
Robot Arm 1 Joint Angles: [-0.00246152057101651;2.12964637035142;-0.00246152057102295;-0.00729497513414104;-0.00246152057102295;-2.04377427531742;-0.00246152047053394]
Mobile Platform 2 Pose: [-2.36235896819767;9.97281409765895;6.37955996543971]
Robot Arm 2 Joint Angles: [0.00256853631131913;-2.08330429763335;0.00256853481712627;0.00650174617642285;0.00256853481716323;1.97424333628495;0.00256853550834284]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.7902902243614;-9.97359580832747;-6.64798666823759]
Robot Arm 1 Joint Angles: [-0.00246153401405555;1.86895744937245;-0.00246153401406201;-0.00729494328713268;-0.00246153401406201;-1.78308541803247;-0.002461533913573]
Mobile Platform 2 Pose: [-2.36235896818127;9.97377201355196;6.38088947172486]
Robot Arm 2 Joint Angles: [0.00256855310812817;-1.93003770528415;0.0025685516139353;0.00650169670869849;0.00256855161397226;1.8209768428712;0.00256855230515187]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.790290224353151;-9.97165958716752;-6.65794863674819]
Robot Arm 1 Joint Angles: [-0.00246154246088644;2.17875281449593;-0.00246154246089288;-0.00729492281927236;-0.00246154246089288;-2.09288082409167;-0.00246154236040386]
Mobile Platform 2 Pose: [-2.36235896817103;9.97207971217132;6.37828513162101]
Robot Arm 2 Joint Angles: [0.00256856359603511;-2.20080589487203;0.00256856210184225;0.00650166539429433;0.00256856210187921;2.09174509508789;0.00256856279305882]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.79029022434826;-9.97340627530686;-6.64887183218581]
Robot Arm 1 Joint Angles: [-0.00246154746976677;1.89928270038109;-0.00246154746977323;-0.00729491099901994;-0.00246154746977323;-1.81341073361733;-0.00246154736928421]
Mobile Platform 2 Pose: [-2.36235896816497;9.97400795437189;6.38134923028252]
Robot Arm 2 Joint Angles: [0.00256856979957498;-1.89228712468578;0.00256856830538211;0.00650164730012536;0.00256856830541908;1.78322636108997;0.00256856899659868]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.790290224345226;-9.97238834235083;-6.65431397126915]
Robot Arm 1 Joint Angles: [-0.00246155057625305;2.06215196583971;-0.0024615505762595;-0.00729490349243403;-0.0024615505762595;-1.97628001408913;-0.00246155047577048]
Mobile Platform 2 Pose: [-2.36235896816118;9.97227506902719;6.3784508580705]
Robot Arm 2 Joint Angles: [0.00256857367999082;-2.16954876821075;0.00256857218579796;0.00650163567295413;0.00256857218583492;2.06048802786928;0.00256857287701453]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.790290224343383;-9.97251220467601;-6.6534712630643]
Robot Arm 1 Joint Angles: [-0.00246155246295107;2.04233398931727;-0.00246155246295752;-0.00729489899794525;-0.00246155246295752;-1.95646204655566;-0.0024615523624685]
Mobile Platform 2 Pose: [-2.36235896815892;9.97349617086735;6.38055463537929]
Robot Arm 2 Joint Angles: [0.00256857599879494;-1.97417246700569;0.00256857450460208;0.00650162889273272;0.00256857450463904;1.86511174022467;0.00256857519581865]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.790290224342275;-9.97332888079225;-6.64945281482647]
Robot Arm 1 Joint Angles: [-0.00246155359832064;1.91166580800925;-0.00246155359832709;-0.00729489628874509;-0.00246155359832709;-1.82579387066605;-0.00246155349783808]
Mobile Platform 2 Pose: [-2.36235896815753;9.97318919567538;6.37982563055662]
Robot Arm 2 Joint Angles: [0.00256857741831889;-2.02328849349889;0.00256857592412603;0.00650162467158748;0.00256857592416299;1.91422777516015;0.0025685766153426]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.790290224341582;-9.97169930637197;-6.65771379730269]
Robot Arm 1 Joint Angles: [-0.00246155430763763;2.17239771354247;-0.00246155430764407;-0.00729489457666021;-0.00246155430764407;-2.08652577962344;-0.00246155420715505]
Mobile Platform 2 Pose: [-2.36235896815668;9.97254470938905;6.3790048966376]
Robot Arm 2 Joint Angles: [0.00256857829034656;-2.12640629673039;0.0025685767961537;0.00650162208993439;0.00256857679619066;2.01734558355496;0.00256857748737027]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.790290224341172;-9.9736177813541;-6.64785966244156]
Robot Arm 1 Joint Angles: [-0.00246155472716933;1.86544171541019;-0.00246155472717579;-0.00729489358613569;-0.00246155472717579;-1.7795697834722;-0.00246155462668677]
Mobile Platform 2 Pose: [-2.36235896815617;9.97391522459437;6.38105778162728]
Robot Arm 2 Joint Angles: [0.00256857881024543;-1.90712386235295;0.00256857731605257;0.00650162056436133;0.00256857731608953;1.79806315222867;0.00256857800726914]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.790290224340915;-9.97187540273219;-6.65693947163588]
Robot Arm 1 Joint Angles: [-0.00246155499021707;2.14422229427791;-0.00246155499022351;-0.00729489294852812;-0.00246155499022351;-2.05835036361514;-0.0024615548897345]
Mobile Platform 2 Pose: [-2.36235896815585;9.97202797852949;6.37809060822334]
Robot Arm 2 Joint Angles: [0.0025685791359482;-2.2090832317591;0.00256857764175534;0.0065016195898017;0.00256857764179231;2.10002252358394;0.00256857833297191]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.790290224340762;-9.9730791730057;-6.65059609301087]
Robot Arm 1 Joint Angles: [-0.00246155514741571;1.95161905014512;-0.00246155514742217;-0.00729489257655381;-0.00246155514742217;-1.8657471202263;-0.00246155504693315]
Mobile Platform 2 Pose: [-2.36235896815566;9.97393580246897;6.38118034426506]
Robot Arm 2 Joint Angles: [0.00256857932866031;-1.90383140088098;0.00256857783446745;0.00650161902807407;0.00256857783450441;1.79477069382928;0.00256857852568402]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.790290224340668;-9.97283566002929;-6.65207625157847]
Robot Arm 1 Joint Angles: [-0.00246155524369468;1.99058112613881;-0.00246155524370113;-0.00729489234499588;-0.00246155524370113;-1.9047091966831;-0.00246155514321211]
Mobile Platform 2 Pose: [-2.36235896815555;9.97249704485785;6.37870266564018]
Robot Arm 2 Joint Angles: [0.00256857944870779;-2.1340326183008;0.00256857795451493;0.00650161866875957;0.00256857795455189;2.02497191196772;0.0025685786457315]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036829e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.79029022434061;-9.97208304302649;-6.65574941041524]
Robot Arm 1 Joint Angles: [-0.00246155530297945;2.11099984644534;-0.0024615553029859;-0.00729489220290013;-0.0024615553029859;-2.02512791727382;-0.00246155520249688]
Mobile Platform 2 Pose: [-2.36235896815548;9.97324094344003;6.38008001795888]
Robot Arm 2 Joint Angles: [0.00256857952093801;-2.01500884494014;0.00256857802674515;0.0065016184569438;0.00256857802678211;1.90594813903069;0.00256857871796172]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.810920e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.637487e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.790290224340575;-9.97355589804733;-6.64829067508782]
Robot Arm 1 Joint Angles: [-0.00246155533828082;1.87534304302623;-0.00246155533828727;-0.00729489211914406;-0.00246155533828727;-1.78947111402222;-0.00246155523779826]
Mobile Platform 2 Pose: [-2.36235896815543;9.97345030709023;6.38018866727586]
Robot Arm 2 Joint Angles: [0.00256857956479307;-1.98151066077843;0.0025685780706002;0.00650161832701445;0.00256857807063717;1.87244995512884;0.00256857876181677]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563675e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.657677e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.790290224340554;-9.97163424154888;-6.65815453252547]
Robot Arm 1 Joint Angles: [-0.0024615553604538;2.18280808272459;-0.00246155536046024;-0.00729489206544733;-0.00246155536046024;-2.09693615382797;-0.00246155525997123]
Mobile Platform 2 Pose: [-2.36235896815541;9.97231294750066;6.37855715089716]
Robot Arm 2 Joint Angles: [0.00256857959193972;-2.16348819502994;0.00256857809774686;0.0065016182463441;0.00256857809778382;2.0544274895417;0.00256857878896343]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056006e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.234298e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.790290224340541;-9.97348102354384;-6.64859236009776]
Robot Arm 1 Joint Angles: [-0.00246155537358211;1.88732296350007;-0.00246155537358857;-0.00729489203447619;-0.00246155537358857;-1.8014510346654;-0.00246155527309955]
Mobile Platform 2 Pose: [-2.36235896815539;9.97399892932519;6.38116361075106]
Robot Arm 2 Joint Angles: [0.00256857960804913;-1.89373110305656;0.00256857811385627;0.00650161819921692;0.00256857811389323;1.78467039766257;0.00256857880507284]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231707e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.576182e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.790290224340533;-9.97225080421868;-6.65511501930747]
Robot Arm 1 Joint Angles: [-0.00246155538175161;2.08415805550594;-0.00246155538175806;-0.00729489201471364;-0.00246155538175806;-1.99828612671079;-0.00246155528126904]
Mobile Platform 2 Pose: [-2.36235896815538;9.97206326898885;6.37804903633136]
Robot Arm 2 Joint Angles: [0.00256857961815197;-2.20343675684223;0.00256857812395911;0.00650161816894915;0.00256857812399607;2.09437605150877;0.00256857881517568]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531954e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.877843e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.790290224340528;-9.97265434078093;-6.65284350744571]
Robot Arm 1 Joint Angles: [-0.00246155538669207;2.01959220553373;-0.00246155538669852;-0.00729489200296606;-0.00246155538669852;-1.93372027676209;-0.00246155528620951]
Mobile Platform 2 Pose: [-2.36235896815538;9.9738035927485;6.38091980590136]
Robot Arm 2 Joint Angles: [0.00256857962414423;-1.92498495527935;0.00256857812995136;0.00650161815147493;0.00256857812998832;1.81592424998084;0.00256857882116793]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692341e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.713474e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.790290224340525;-9.97322813619594;-6.65009202118817]
Robot Arm 1 Joint Angles: [-0.00246155538967707;1.9277849391259;-0.00246155538968353;-0.00729489199582948;-0.00246155538968353;-1.84191301036853;-0.00246155528919451]
Mobile Platform 2 Pose: [-2.36235896815537;9.9727609570327;6.37903746752497]
Robot Arm 2 Joint Angles: [0.002568579627855;-2.09180666979734;0.00256857813366214;0.00650161814039019;0.0025685781336991;1.982745964521;0.00256857882487871]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663786e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.580650e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.790290224340524;-9.97176377606782;-6.657478691002]
Robot Arm 1 Joint Angles: [-0.00246155539153674;2.16208255962123;-0.00246155539154319;-0.00729489199134725;-0.00246155539154319;-2.07621063087282;-0.00246155529105417]
Mobile Platform 2 Pose: [-2.36235896815537;9.97296264458275;6.379569677699]
Robot Arm 2 Joint Angles: [0.00256857963010559;-2.05953666178302;0.00256857813591273;0.00650161813376688;0.00256857813594969;1.95047595651993;0.0025685788271293]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984360e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.084368e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.790290224340523;-9.97362810011697;-6.64792861102124]
Robot Arm 1 Joint Angles: [-0.00246155539263748;1.86379071175456;-0.00246155539264394;-0.00729489198874634;-0.00246155539264394;-1.77791878301135;-0.00246155529215492]
Mobile Platform 2 Pose: [-2.36235896815537;9.97367371499602;6.38050690762208]
Robot Arm 2 Joint Angles: [0.00256857963146117;-1.94576539565422;0.00256857813726831;0.00650161812976646;0.00256857813730527;1.83670469039913;0.00256857882848488]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059042e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.393484e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.790290224340522;-9.97178688159041;-6.65748621771393]
Robot Arm 1 Joint Angles: [-0.0024615553933286;2.15838567600207;-0.00246155539333504;-0.00729489198707081;-0.00246155539333504;-2.07251374726221;-0.00246155529284602]
Mobile Platform 2 Pose: [-2.36235896815537;9.97214070207539;6.37820000320879]
Robot Arm 2 Joint Angles: [0.00256857963230553;-2.19104746295381;0.00256857813811267;0.00650161812724904;0.00256857813814963;2.08198675770376;0.00256857882932924]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857051e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.433617e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.790290224340521;-9.97319546015197;-6.65010852803472]
Robot Arm 1 Joint Angles: [-0.00246155539374035;1.93301310615054;-0.00246155539374681;-0.00729489198609762;-0.00246155539374681;-1.84714117741262;-0.00246155529325779]
Mobile Platform 2 Pose: [-2.36235896815537;9.97402115683999;6.38116840501317]
Robot Arm 2 Joint Angles: [0.0025685796328052;-1.89017470061535;0.00256857813861234;0.00650161812579055;0.0025685781386493;1.78111399536821;0.00256857882982891]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905716e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.166200e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.790290224340521;-9.97269674216055;-6.6529012601946]
Robot Arm 1 Joint Angles: [-0.00246155539399362;2.0128079847777;-0.00246155539400007;-0.00729489198548747;-0.00246155539400007;-1.926936056041;-0.00246155529351106]
Mobile Platform 2 Pose: [-2.36235896815537;9.97218213627543;6.37813909003232]
Robot Arm 2 Joint Angles: [0.00256857963311824;-2.18441799094456;0.00256857813892538;0.00650161812485232;0.00256857813896235;2.0753572856993;0.00256857883014195]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634540e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.589084e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.790290224340521;-9.97221078245804;-6.65519624951457]
Robot Arm 1 Joint Angles: [-0.00246155539414892;2.09056153717925;-0.00246155539415536;-0.00729489198511596;-0.00246155539415536;-2.00468960844331;-0.00246155529366635]
Mobile Platform 2 Pose: [-2.36235896815537;9.97361542667696;6.38056376329501]
Robot Arm 2 Joint Angles: [0.00256857963330472;-1.95509152669962;0.00256857813911185;0.00650161812430776;0.00256857813914881;1.84603082145544;0.00256857883032842]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.066851e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.537689e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.790290224340521;-9.97350061195453;-6.64870180537773]
Robot Arm 1 Joint Angles: [-0.00246155539424163;1.88418881774069;-0.00246155539424809;-0.00729489198489565;-0.00246155539424809;-1.79831688900518;-0.00246155529375907]
Mobile Platform 2 Pose: [-2.36235896815537;9.97304216496421;6.37941288961736]
Robot Arm 2 Joint Angles: [0.00256857963341936;-2.0468134007392;0.0025685781392265;0.00650161812396623;0.00256857813926346;1.93775269549571;0.00256857883044307]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.068964e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.716972e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716309425291;-6.65826475779724]
Robot Arm 1 Joint Angles: [-0.00246155539429984;2.18333592580918;-0.00246155539430628;-0.00729489198475474;-0.00246155539430628;-2.09746399707396;-0.00246155529381727]
Mobile Platform 2 Pose: [-2.36235896815537;9.97268375952547;6.37905524360944]
Robot Arm 2 Joint Angles: [0.00256857963348948;-2.10415827093715;0.00256857813929662;0.00650161812375905;0.00256857813933359;1.99509756569407;0.00256857883051319]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.775890e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.479193e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.790290224340521;-9.97354092047776;-6.64840134113329]
Robot Arm 1 Joint Angles: [-0.00246155539433423;1.87773945402366;-0.00246155539434069;-0.00729489198467367;-0.00246155539434069;-1.7918675252886;-0.00246155529385168]
Mobile Platform 2 Pose: [-2.36235896815537;9.97384676969123;6.38075340774476]
Robot Arm 2 Joint Angles: [0.00256857963353141;-1.91807664441458;0.00256857813933855;0.00650161812363582;0.00256857813937551;1.80901593917175;0.00256857883055512]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247117e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.696738e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.790290224340521;-9.97212037976123;-6.65588297852306]
Robot Arm 1 Joint Angles: [-0.00246155539435574;2.10502596866848;-0.00246155539436218;-0.00729489198462157;-0.00246155539436218;-2.01915403993352;-0.00246155529387316]
Mobile Platform 2 Pose: [-2.36235896815537;9.97204457094842;6.3779577939877]
Robot Arm 2 Joint Angles: [0.00256857963355766;-2.20642844326364;0.0025685781393648;0.00650161812355732;0.00256857813940176;2.09736773802097;0.00256857883058137]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064252e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.916517e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.790290224340521;-9.97279446107357;-6.65223074443431]
Robot Arm 1 Joint Angles: [-0.00246155539436866;1.99717295869341;-0.00246155539437511;-0.0072948919845909;-0.00246155539437511;-1.91130102995851;-0.0024615552938861]
Mobile Platform 2 Pose: [-2.36235896815537;9.9739815232568;6.3810677438764]
Robot Arm 2 Joint Angles: [0.00256857963357316;-1.89651607392321;0.0025685781393803;0.00650161812351218;0.00256857813941726;1.78745536868063;0.00256857883059687]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.743017e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.747329e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.790290224340521;-9.9731152454201;-6.6507936527353]
Robot Arm 1 Joint Angles: [-0.00246155539437651;1.94584746324843;-0.00246155539438296;-0.00729489198457212;-0.00246155539438296;-1.85997553451357;-0.00246155529389394]
Mobile Platform 2 Pose: [-2.36235896815537;9.97237312053056;6.37834597910896]
Robot Arm 2 Joint Angles: [0.00256857963358286;-2.15386051012144;0.00256857813939;0.00650161812348308;0.00256857813942696;2.04479980487891;0.00256857883060657]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155937e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.591903e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.790290224340521;-9.97184705378916;-6.6571498474161]
Robot Arm 1 Joint Angles: [-0.0024615553943814;2.14875812419855;-0.00246155539438784;-0.00729489198456031;-0.00246155539438784;-2.06288619546371;-0.00246155529389883]
Mobile Platform 2 Pose: [-2.36235896815537;9.97337986098482;6.38012710173536]
Robot Arm 2 Joint Angles: [0.00256857963358866;-1.99278203744105;0.0025685781393958;0.00650161812346613;0.00256857813943276;1.88372133219856;0.00256857883061237]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302642e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.009775e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362257667489;-6.64808147597899]
Robot Arm 1 Joint Angles: [-0.00246155539438426;1.86467446248175;-0.00246155539439072;-0.00729489198455359;-0.00246155539439072;-1.77880253374693;-0.00246155529390171]
Mobile Platform 2 Pose: [-2.36235896815537;9.9733160677595;6.37979139339336]
Robot Arm 2 Joint Angles: [0.00256857963359221;-2.00298895349187;0.00256857813939934;0.00650161812345561;0.0025685781394363;1.89392824824941;0.00256857883061591]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531303e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.593649e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.790290224340521;-9.97171662242301;-6.65793912132949]
Robot Arm 1 Joint Angles: [-0.00246155539438611;2.16962714278287;-0.00246155539439255;-0.00729489198454907;-0.00246155539439255;-2.08375521404806;-0.00246155529390353]
Mobile Platform 2 Pose: [-2.36235896815537;9.97242924710553;6.37857555352266]
Robot Arm 2 Joint Angles: [0.00256857963359439;-2.14488025812685;0.00256857813940153;0.00650161812344909;0.0025685781394385;2.0358195528844;0.0025685788306181]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.612856e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.750287e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.790290224340521;-9.9733000174627;-6.6496828181616]
Robot Arm 1 Joint Angles: [-0.00246155539438716;1.91628393643298;-0.00246155539439362;-0.00729489198454662;-0.00246155539439362;-1.83041200769817;-0.0024615552939046]
Mobile Platform 2 Pose: [-2.36235896815537;9.97396232583977;6.38091144127675]
Robot Arm 2 Joint Angles: [0.00256857963359568;-1.89958766064761;0.00256857813940282;0.00650161812344536;0.00256857813943978;1.79052695540516;0.00256857883061939]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408487e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.794153e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.790290224340521;-9.97255494292001;-6.65373770049828]
Robot Arm 1 Joint Angles: [-0.00246155539438784;2.03549586326279;-0.00246155539439429;-0.00729489198454497;-0.00246155539439429;-1.94962393452799;-0.00246155529390527]
Mobile Platform 2 Pose: [-2.36235896815537;9.97203382048595;6.3778458234549]
Robot Arm 2 Joint Angles: [0.00256857963359651;-2.20814851725914;0.00256857813940365;0.00650161812344283;0.00256857813944061;2.09908781201669;0.00256857883062022]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394166e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.404073e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.790290224340521;-9.9723465089601;-6.65460325164364]
Robot Arm 1 Joint Angles: [-0.00246155539438825;2.06884529684863;-0.0024615553943947;-0.00729489198454398;-0.0024615553943947;-1.98297336811383;-0.00246155529390568]
Mobile Platform 2 Pose: [-2.36235896815537;9.97388074574215;6.38086172229229]
Robot Arm 2 Joint Angles: [0.00256857963359698;-1.91264047626748;0.00256857813940412;0.00650161812344151;0.00256857813944108;1.80357977102504;0.00256857883062069]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956261e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.267105e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.790290224340521;-9.97343026721835;-6.64919006595864]
Robot Arm 1 Joint Angles: [-0.00246155539438848;1.89544397552845;-0.00246155539439493;-0.00729489198454347;-0.00246155539439493;-1.80957204679365;-0.00246155529390592]
Mobile Platform 2 Pose: [-2.36235896815537;9.97261783942221;6.37864509654305]
Robot Arm 2 Joint Angles: [0.00256857963359729;-2.11470548745813;0.00256857813940443;0.00650161812344055;0.00256857813944139;2.0056447822157;0.002568578830621]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260393e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.904398e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.790290224340521;-9.97164968922832;-6.65826291488905]
Robot Arm 1 Joint Angles: [-0.00246155539438866;2.18033645393325;-0.0024615553943951;-0.00729489198454299;-0.0024615553943951;-2.09446452519845;-0.00246155529390608]
Mobile Platform 2 Pose: [-2.36235896815537;9.97311122603727;6.37963399581871]
Robot Arm 2 Joint Angles: [0.00256857963359747;-2.03576362904755;0.0025685781394046;0.00650161812344005;0.00256857813944157;1.92670292380511;0.00256857883062117]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807368e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.447023e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358545581044;-6.64829120789055]
Robot Arm 1 Joint Angles: [-0.00246155539438872;1.87061380079364;-0.00246155539439518;-0.0072948919845429;-0.00246155539439518;-1.78474187205884;-0.00246155529390616]
Mobile Platform 2 Pose: [-2.36235896815537;9.97356136245725;6.38013833832806]
Robot Arm 2 Joint Angles: [0.00256857963359757;-1.96374180185154;0.00256857813940471;0.00650161812343975;0.00256857813944167;1.8546810966091;0.00256857883062128]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620232e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.201343e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.790290224340521;-9.97200013263575;-6.65659636570093]
Robot Arm 1 Joint Angles: [-0.0024615553943888;2.12426550874398;-0.00246155539439524;-0.00729489198454266;-0.00246155539439524;-2.03839358000917;-0.00246155529390622]
Mobile Platform 2 Pose: [-2.36235896815537;9.97222309363386;6.37816763607215]
Robot Arm 2 Joint Angles: [0.00256857963359765;-2.17786481359279;0.00256857813940479;0.00650161812343948;0.00256857813944175;2.06880410835035;0.00256857883062136]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640565e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.249470e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.790290224340521;-9.97292968868563;-6.6516439844271]
Robot Arm 1 Joint Angles: [-0.00246155539438882;1.97553654076372;-0.00246155539439527;-0.00729489198454264;-0.00246155539439527;-1.88966461202892;-0.00246155529390625]
Mobile Platform 2 Pose: [-2.36235896815537;9.974017143156;6.38097081471022]
Robot Arm 2 Joint Angles: [0.00256857963359768;-1.89081689005125;0.00256857813940481;0.00650161812343945;0.00256857813944178;1.78175618480882;0.00256857883062138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711079e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.859045e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.790290224340521;-9.97299183231991;-6.6515465247639]
Robot Arm 1 Joint Angles: [-0.00246155539438884;1.96559355927804;-0.00246155539439529;-0.00729489198454259;-0.00246155539439529;-1.87972163054324;-0.00246155529390627]
Mobile Platform 2 Pose: [-2.36235896815537;9.97210948668422;6.37786775551438]
Robot Arm 2 Joint Angles: [0.00256857963359771;-2.19604192553542;0.00256857813940486;0.00650161812343928;0.00256857813944182;2.08698122029298;0.00256857883062143]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373293e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.080862e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.790290224340521;-9.97194709861065;-6.65673630634595]
Robot Arm 1 Joint Angles: [-0.00246155539438887;2.13275095275972;-0.00246155539439531;-0.0072948919845425;-0.00246155539439531;-2.04687902402492;-0.00246155529390629]
Mobile Platform 2 Pose: [-2.36235896815537;9.97372138163146;6.38055494183713]
Robot Arm 2 Joint Angles: [0.00256857963359772;-1.93813873397823;0.00256857813940486;0.00650161812343932;0.00256857813944182;1.82907802873579;0.00256857883062143]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767657e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.425121e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360123849484;-6.64831693464986]
Robot Arm 1 Joint Angles: [-0.00246155539438885;1.86808857128934;-0.00246155539439531;-0.00729489198454258;-0.00246155539439531;-1.78221664255454;-0.00246155529390629]
Mobile Platform 2 Pose: [-2.36235896815537;9.97289307107796;6.37900384445299]
Robot Arm 2 Joint Angles: [0.00256857963359773;-2.07066842253764;0.00256857813940487;0.00650161812343925;0.00256857813944183;1.9616077172952;0.00256857883062144]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002359e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.664441e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.790290224340521;-9.97166632766659;-6.65828943007524]
Robot Arm 1 Joint Angles: [-0.00246155539438888;2.17767430380964;-0.00246155539439532;-0.00729489198454245;-0.00246155539439532;-2.09180237507484;-0.0024615552939063]
Mobile Platform 2 Pose: [-2.36235896815537;9.97282961204288;6.37911701074085]
Robot Arm 2 Joint Angles: [0.00256857963359774;-2.08082186815031;0.00256857813940488;0.00650161812343923;0.00256857813944184;1.97176116290788;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.186969e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.017967e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.790290224340521;-9.97339148640314;-6.64932581610123]
Robot Arm 1 Joint Angles: [-0.00246155539438886;1.90164890596131;-0.00246155539439532;-0.00729489198454255;-0.00246155539439532;-1.81577697722651;-0.0024615552939063]
Mobile Platform 2 Pose: [-2.36235896815537;9.97376222505781;6.38042532874317]
Robot Arm 2 Joint Angles: [0.00256857963359774;-1.93160378576142;0.00256857813940487;0.00650161812343927;0.00256857813944184;1.82254308051898;0.00256857883062144]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546737e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.203515e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.790290224340521;-9.97241328234063;-6.65456995766314]
Robot Arm 1 Joint Angles: [-0.00246155539438888;2.05816155596407;-0.00246155539439532;-0.00729489198454249;-0.00246155539439532;-1.97228962722926;-0.00246155529390631]
Mobile Platform 2 Pose: [-2.36235896815537;9.97208511280286;6.37786212872889]
Robot Arm 2 Joint Angles: [0.00256857963359775;-2.19994174655374;0.00256857813940489;0.00650161812343917;0.00256857813944185;2.09088104131131;0.00256857883062146]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.135947e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.045261e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.790290224340521;-9.97248703698016;-6.65398654778417]
Robot Arm 1 Joint Angles: [-0.00246155539438888;2.04636081363886;-0.00246155539439532;-0.00729489198454249;-0.00246155539439532;-1.96048888490406;-0.00246155529390631]
Mobile Platform 2 Pose: [-2.36235896815537;9.97401016083469;6.38092607608951]
Robot Arm 2 Joint Angles: [0.00256857963359774;-1.89193406146045;0.00256857813940488;0.00650161812343927;0.00256857813944184;1.78287335621802;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235397e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.395604e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.790290224340521;-9.9733454663867;-6.64975164370939]
Robot Arm 1 Joint Angles: [-0.00246155539438886;1.9090121085914;-0.00246155539439532;-0.00729489198454254;-0.00246155539439532;-1.8231401798566;-0.0024615552939063]
Mobile Platform 2 Pose: [-2.36235896815537;9.9722642809103;6.37801540929088]
Robot Arm 2 Joint Angles: [0.00256857963359775;-2.17127484936331;0.00256857813940489;0.00650161812343918;0.00256857813944185;2.06221414412087;0.00256857883062146]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.017666e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.910005e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.790290224340521;-9.97169001706291;-6.658151540421]
Robot Arm 1 Joint Angles: [-0.00246155539438889;2.17388400039864;-0.00246155539439533;-0.00729489198454243;-0.00246155539439533;-2.08801207166384;-0.00246155529390631]
Mobile Platform 2 Pose: [-2.36235896815537;9.97350943984669;6.38015841708866]
Robot Arm 2 Joint Angles: [0.00256857963359774;-1.97204941954154;0.00256857813940488;0.00650161812343924;0.00256857813944184;1.8629887142991;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924095e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.369090e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361436228705;-6.64826332607305]
Robot Arm 1 Joint Angles: [-0.00246155539438886;1.86598876453623;-0.00246155539439532;-0.00729489198454256;-0.00246155539439532;-1.78011683580143;-0.0024615552939063]
Mobile Platform 2 Pose: [-2.36235896815537;9.97317377271031;6.37938532659269]
Robot Arm 2 Joint Angles: [0.00256857963359775;-2.0257561613621;0.00256857813940488;0.00650161812343922;0.00256857813944185;1.91669545611966;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213659e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.941816e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.790290224340521;-9.97189295714165;-6.65724083491554]
Robot Arm 1 Joint Angles: [-0.00246155539438889;2.14141358779996;-0.00246155539439533;-0.00729489198454245;-0.00246155539439533;-2.05554165906515;-0.00246155529390631]
Mobile Platform 2 Pose: [-2.36235896815537;9.97255908862965;6.37861410830307]
Robot Arm 2 Joint Angles: [0.00256857963359775;-2.1241056142674;0.00256857813940489;0.00650161812343919;0.00256857813944185;2.01504490902497;0.00256857883062146]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673194e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.075749e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.790290224340521;-9.9730574715914;-6.65109640830708]
Robot Arm 1 Joint Angles: [-0.00246155539438887;1.95509127583953;-0.00246155539439532;-0.00729489198454252;-0.00246155539439532;-1.86921934710473;-0.0024615552939063]
Mobile Platform 2 Pose: [-2.36235896815537;9.97390875582087;6.38063172572403]
Robot Arm 2 Joint Angles: [0.00256857963359774;-1.90815886367179;0.00256857813940488;0.00650161812343926;0.00256857813944184;1.79909815842935;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183014e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.596678e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.790290224340521;-9.97285991343134;-6.65233987590714]
Robot Arm 1 Joint Angles: [-0.00246155539438887;1.98670058144886;-0.00246155539439532;-0.00729489198454251;-0.00246155539439532;-1.90082865271406;-0.00246155529390631]
Mobile Platform 2 Pose: [-2.36235896815537;9.97202861289291;6.37768017063248]
Robot Arm 2 Joint Angles: [0.00256857963359775;-2.20898173214522;0.0025685781394049;0.00650161812343916;0.00256857813944186;2.09992102690279;0.00256857883062147]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.344477e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.113348e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.790290224340521;-9.97206147105343;-6.65625050635088]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.0943951023932;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.02857943131825;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36235896815537;9.9739414958035;6.38077543377186]
Robot Arm 2 Joint Angles: [0.00256857963359774;-1.90292046645065;0.00256857813940488;0.00650161812343926;0.00256857813944184;1.79385976120822;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.852968e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.821282e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.790290224340521;-9.97356419643189;-6.64863399116669]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.85395904183944;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.7881433707645;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36235896815537;9.97248327941047;6.37826946093987]
Robot Arm 2 Joint Angles: [0.00256857963359775;-2.13623508933638;0.00256857813940489;0.00650161812343918;0.00256857813944185;2.02717438409394;0.00256857883062146]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.230101e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.076416e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163376794868;-6.65855403179913]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16282759915271;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09701192807777;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36235896815537;9.97325610015438;6.37969153541259]
Robot Arm 2 Joint Angles: [0.00256857963359775;-2.01258377031022;0.00256857813940488;0.00650161812343922;0.00256857813944185;1.90352306506778;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.103692e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.631398e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.790290224340521;-9.97343893141992;-6.64919532797613]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87400144375469;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80818577267975;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36235896815537;9.97343653307229;6.37975294509438]
Robot Arm 2 Joint Angles: [0.00256857963359775;-1.98371450344558;0.00256857813940488;0.00650161812343923;0.00256857813944184;1.87465379820315;0.00256857883062145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.041615e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.494558e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.790290224340521;-9.97233458471323;-6.65508051166471]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.05069691682551;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.98488124575056;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97232459313831;6.37816396753167]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0943951023932;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.05256418577703;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.732460e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.032786e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.790290224340521;-9.97253107068539;-6.65386884054769]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01925916127891;-0.00246155353175018;-0.00729489384718764;-0.00246155353175018;-1.95344349020397;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97399588257557;6.38074414254624]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82698879243154;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78515787581537;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.574850e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.377118e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.790290224340521;-9.97331945972091;-6.64999951293311]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89311691559632;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82730124452137;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97206757249784;6.37762800110771]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.13551840486857;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.0936874882524;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.468717e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.911310e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.790290224340521;-9.97168650301059;-6.65827969299103]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15438998924785;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08857431817291;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97369399581456;6.38033555759476]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.87529067419269;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.83345975757653;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.717132e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.021573e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361036446588;-6.64839435056415]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84657215640136;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78075648532641;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97295677112702;6.37892634516826]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.99324662419852;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95141570758236;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.147755e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.538151e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.790290224340521;-9.97190476091683;-6.65729428834906]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11946872424838;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.05365305317343;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97265479430128;6.37865707367601]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.04156291631684;0.00256857627675974;0.00650161998608434;0.0025685762767967;1.99973199970068;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.547489e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.436074e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.790290224340521;-9.97300882941202;-6.65145657001468]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.94281776501924;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.8770020939443;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97387187431897;6.38044721742162]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84683011348668;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.80499919687052;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.648229e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.737029e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.790290224340521;-9.97291685451351;-6.65215548845722]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.95753374878077;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.89171807770582;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200516200477;6.37752346684693]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.14550408375905;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10367316714288;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.062073e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.641735e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.790290224340521;-9.97198117401527;-6.65677869529107]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.10724262849855;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.0414269574236;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97390480739113;6.38060428066023]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84156082194146;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.79972990532529;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.287910e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.841864e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.790290224340521;-9.97359074947939;-6.64859618702464]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.8497105542391;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78389488316416;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97258483646258;6.37830214554436]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05275617050912;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01092525389296;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.602539e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.742992e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716545616061;-6.65856939120107]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15950061396652;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09368494289158;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97303106807051;6.37921555548163]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.98135911324072;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.93952819662455;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.957717e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.090946e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.790290224340521;-9.97338360988037;-6.64958663100458]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88285289008223;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81703721900728;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97364125155742;6.37998454218795]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.88372975533442;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84189883871826;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.338558e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.488166e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.790290224340521;-9.97243055687622;-6.65470279777819]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03534137074599;-0.00246155353175018;-0.00729489384718763;-0.00246155353175018;-1.96952569967105;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97209734844687;6.37765758763962]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.13075425302282;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.08892333640666;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.660504e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.485316e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.790290224340521;-9.97243379917853;-6.65448168684159]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03482260237744;-0.00246155353175018;-0.00729489384718763;-0.00246155353175018;-1.9690069313025;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97400693574483;6.38068636651553]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82522028534928;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78338936873312;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.984439e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.454542e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.790290224340521;-9.97338165887696;-6.6497796359533]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88316505062784;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81734937955289;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97227037822561;6.37778814753391]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.10306948842495;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.06123857180879;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.918141e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.490566e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.790290224340521;-9.97165543437654;-6.65855896626748]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15936097069582;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09354529962088;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97339497973043;6.37974736725967]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.92313324765411;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.88130233103795;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.163013e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.039175e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.790290224340521;-9.97359151357461;-6.64859379431819]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84958829900412;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78377262792918;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97332262688255;6.37939738320075]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.93470970331395;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.89287878669778;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.261986e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.944959e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.790290224340521;-9.97197861205907;-6.6570359080521]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.10765254149109;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04183687041615;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97232853215966;6.37800272753373]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.09376485897613;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.05193394235996;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.818006e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.294425e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.790290224340521;-9.9729199066754;-6.65202435829351]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.95704540287755;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.8912297318026;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97399496328944;6.38057421959082]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82713587821163;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78530496159547;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.216098e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.555968e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.790290224340521;-9.97300594797255;-6.65180338085253]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.94327879533369;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.87746312425875;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97206571672567;6.3774578700469]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.13581532841385;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.09398441179768;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.637693e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.164361e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.790290224340521;-9.97190713359953;-6.6572753534779]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11908909501711;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.05327342394217;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97369754885142;6.38017325701197]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.87472218829473;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.83289127167856;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.117473e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.375424e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360988949499;-6.64859682233506]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84664815174349;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78083248066855;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97295160504225;6.37875039537498]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.99407319776169;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95224228114552;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.138098e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.083475e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.790290224340521;-9.97168534729952;-6.65853362102149]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15457490301832;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08875923194337;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97265974580492;6.37849732415745]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.04077067573492;0.00256857627675974;0.00650161998608434;0.0025685762767967;1.99893975911876;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.261781e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.572285e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.790290224340521;-9.97332165847562;-6.65001277570903]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89276511484247;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82694944376752;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97386944131213;6.38027486113802]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.847219394581;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.80538847796484;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.262792e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.893045e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.790290224340521;-9.97252786025998;-6.6543167240743]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01977282934521;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.95395715827027;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200539672774;6.37735659858559]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.14546652808385;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10363561146768;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.070558e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.055251e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.790290224340521;-9.97233778491017;-6.65508851260411]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.05018488531482;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.98436921423987;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97390695609512;6.38043957070885]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84121702930247;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79938611268631;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.452418e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.414498e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.790290224340521;-9.97343726066228;-6.64959303076708]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87426876497679;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80845309390185;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97258006137565;6.3781271245994]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05352018441747;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01168926780131;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.863146e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.968139e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163433945922;-6.65878763698153]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16273615746616;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09692048639122;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9730362308741;6.37905604483562]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.9805330646651;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.93870214804894;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.522515e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.420030e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.790290224340521;-9.97356523689077;-6.64883229164235]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.85379256841905;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78797689734411;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97363746782193;6.37981017296351]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.88433515301338;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84250423639721;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.460101e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.329307e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.790290224340521;-9.97205878196894;-6.65674427372355]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09482535591165;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.02900968483671;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97209963199971;6.37749385106088]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.13038888456863;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.08855796795247;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.325319e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.035863e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.790290224340521;-9.97282753110411;-6.65261034550124]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.9718254942842;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.90600982320926;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97400755372766;6.38051903157527]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82512140809586;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78329049147969;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.420806e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.669126e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.790290224340521;-9.97309091406221;-6.65147100233661]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.92968422098781;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86386854991287;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97226677681057;6.37761518525837]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.10364571483126;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.0618147982151;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.748292e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.047777e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.790290224340521;-9.97184019385736;-6.65773617619858]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12979945376372;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06398378268877;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97339959913598;6.37958689322621]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.92239414276497;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.88056322614881;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.579833e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.376843e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362156160016;-6.64863632858066]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.8447806149171;-0.00246155353175017;-0.00729489384718771;-0.00246155353175017;-1.77896494384216;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97331785639564;6.37922172337342]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.93547298121984;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.89364206460368;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.616657e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.181657e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.790290224340521;-9.9717257805107;-6.65844835283174]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14810558923031;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.08228991815536;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9723324768457;6.37784149347165]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.09313370921001;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.05130279259384;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.301198e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.046104e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.790290224340521;-9.97325339660801;-6.65047206437122]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.9036870136605;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83787134258555;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9739940260785;6.38040426859569]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82728583196235;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78545491534619;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.845590e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.169524e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.790290224340521;-9.97262547707042;-6.65392744233058]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00415413967554;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93833846860059;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97206389405024;6.37728778895262]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.13610695648322;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.09427603986706;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.799454e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.448915e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.790290224340521;-9.97224405878643;-6.65568404867335]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.06518106511329;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.99936539403834;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97370107282911;6.38001090812503]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.87415835186489;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.83232743524873;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.246903e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.261504e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.790290224340521;-9.97348600136501;-6.64944125812205]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86647025254034;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.8006545814654;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97294645580996;6.37857447631292]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.99489707492909;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95306615831293;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.136243e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.262316e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162346487227;-6.65896439041501]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1644760913791;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09866042030415;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97266468940897;6.3783375595882]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0399796990861;0.00256857627675974;0.00650161998608434;0.0025685762767967;1.99814878246993;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.871362e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.194971e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.790290224340521;-9.97353160577988;-6.64910948520695]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8591735461609;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79335787508596;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97386699662876;6.38010248877743]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84761054392106;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.80577962730489;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.314910e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.586963e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.790290224340521;-9.97214436888354;-6.65642384465035]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08113144957511;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01531577850017;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200565951224;6.37718977123473]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.14542448256372;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10359356594756;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.884635e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.405547e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.790290224340521;-9.97273251305632;-6.65321034079554]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98702838193066;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.92121271085572;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97390907762497;6.38027481554912]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84087758452646;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7990466679103;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.399923e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.844404e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.790290224340521;-9.97317109600814;-6.65116185034463]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91685510963888;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.85103943856394;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97257531287945;6.37795214772247]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05427994380951;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01244902719334;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.181717e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.801216e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.790290224340521;-9.97178112101222;-6.65815725098537]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13925110898598;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07343543791104;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97304137411928;6.3788965011002]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97971014543699;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.93787922882083;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.846993e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.524274e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362573261337;-6.64871500381806]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84411325280166;-0.00246155353175017;-0.00729489384718771;-0.00246155353175017;-1.77829758172672;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97363368026614;6.37963580192628]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.88494116194018;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84311024532402;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.188731e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.975555e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.790290224340521;-9.97177540636423;-6.65831577080204]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14016545266433;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07434978158939;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97210193382837;6.37733013975255]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.13002059198284;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.08818967536668;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.571487e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.241766e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.790290224340521;-9.97317921760801;-6.65096241674474]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91555565366014;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.8497399825852;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97400814872127;6.38035165888686]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82502620911956;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7831952925034;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.867412e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.058016e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.790290224340521;-9.97272241649649;-6.65353999691088]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98864383150259;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.92282816042765;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97226320783186;6.37744227386219]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.10421675142512;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.06238583480896;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.087082e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.339017e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.790290224340521;-9.97215364789689;-6.65626305187507]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07964680743991;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01383113636496;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97340419171287;6.37942637460627]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.92165933046282;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.87982841384665;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.845561e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.238371e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.790290224340521;-9.97352767478218;-6.64932558227907]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85980250579224;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.7939868347173;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97331309195884;6.37904607801719]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.936235291108;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.89440437449184;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.630135e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.457702e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162294094539;-6.65908849090597]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1645599196799;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09874424860495;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97233642696785;6.37768026483349]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.09250168966585;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.05067077304968;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.320650e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.769919e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.790290224340521;-9.97349072412878;-6.64942483134452]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86571461033689;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79989893926195;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97399307115792;6.38023428987252]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.8274386192548;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78560770263864;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.575143e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.303813e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.790290224340521;-9.97223441544605;-6.65607937494741]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.06672399957416;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-2.00090832849921;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97206210426242;6.37711775747229]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.13639332253544;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.09456240591928;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.557753e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.634846e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.790290224340521;-9.97263573391105;-6.65381980015842]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00251304517347;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93669737409852;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97370456786446;6.37984851113203]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.87359914620923;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.83176822959306;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.674251e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.850747e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.790290224340521;-9.97324592121152;-6.6508790168994]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90488307709866;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83906740602371;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97294132357744;6.37839858816326]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.995718232132;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95388731551584;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.141282e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.502049e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.790290224340521;-9.97173059337636;-6.65853510691762]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14733553072423;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.08151985964929;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97266962488175;6.37817777961845]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03919002344322;0.00256857627675974;0.00650161998608434;0.0025685762767967;1.99735910682706;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.651284e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.292728e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362238816839;-6.64883302591027]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.8446483639992;-0.00246155353175017;-0.00729489384718771;-0.00246155353175017;-1.77883269292425;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9738645404924;6.37993010071878]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84800352573801;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80617260912184;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.355966e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.630248e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.790290224340521;-9.97183366473659;-6.65813858804411]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1308441130873;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06502844201235;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9720059500651;6.3770229843303]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14537799410653;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10354707749037;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.814089e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.466602e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.790290224340521;-9.97309959496327;-6.65148134149344]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.92829527681842;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86247960574348;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97391117215843;6.38011001548625]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84054245917364;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79871154255747;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.186204e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.756547e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.790290224340521;-9.97281773038386;-6.65315924656267]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97339360952367;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.90757793844872;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97257059101202;6.3777772149177]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05503544259978;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01320452598361;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.741458e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.177039e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.790290224340521;-9.9720675585542;-6.6568203896289]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.0934211022693;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.02760543119436;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97304649769216;6.37873692410627]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97889037377618;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.93705945716002;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.940644e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.997752e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.790290224340521;-9.97356212362608;-6.64924700960606]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85429069076941;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78847501969447;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97362988911754;6.3794614294361]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88554774571683;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84371682910067;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.275285e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.015584e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163278010652;-6.65915978978567]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16298565389788;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09716998282294;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97210425360569;6.37716645321115]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.12964942761235;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.08781851099618;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.273842e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.195771e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.790290224340521;-9.97344273586579;-6.64977756410555]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87339273241467;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80757706133973;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97400872092952;6.38018424880714]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82493465579924;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78310373918307;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.089859e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402293e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.790290224340521;-9.97232792332133;-6.65571584346032]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.05176273952948;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.98594706845454;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97225967119745;6.3772694131567]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.10478261293156;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.0629516963154;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.940764e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.259480e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.790290224340521;-9.9725381351512;-6.65443388274042]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01812884674916;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.95231317567422;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97340875747605;6.37926581143205]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.92092880835534;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.87909789173917;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.660440e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.826936e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.790290224340521;-9.97331490035821;-6.65062518859309]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89384641362702;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82803074255208;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97330833378051;6.37887044743142]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.9369965996409;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.89516568302474;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.715862e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.679061e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.790290224340521;-9.97168919279467;-6.65886675625081]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15395962379389;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08814395271895;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9723403822211;6.37751904115593]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.09186884914645;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.05003793253029;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.819939e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.175116e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361153169483;-6.64899045705395]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84638539976932;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78056972869438;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97399209874224;6.38006428379714]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82759420576442;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78576328914825;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.579040e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.522566e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.790290224340521;-9.97189989677844;-6.65792001628776]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12024698639141;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.05443131531646;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97206034715199;6.37694777525214]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13667446020462;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.09484354358845;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.663614e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.038434e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.790290224340521;-9.97301508646029;-6.65202591809975]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.9418166372946;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.87600096621966;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9737080340754;6.37968606623295]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.87304455245923;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.83121363584307;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.709216e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.298006e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.790290224340521;-9.97291052711791;-6.65278979430786]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.95854613207578;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.89273046100084;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97293620848937;6.378222731103]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.9965366462242;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95470572960804;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.275950e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.916382e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.790290224340521;-9.97198676009713;-6.6573511204064]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1063488554001;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04053318432515;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97267455199463;6.37801798390317]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03840168538281;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99657076876665;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.347344e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.506994e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358923122312;-6.64920632622062]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84995347524126;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78413780416631;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97386207312581;6.37975769733913]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84839830439347;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80656738777731;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.007063e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.410104e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.790290224340521;-9.97165287658108;-6.65917872707754]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15977021796927;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09395454689432;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200626809335;6.37685623740887]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14532710958636;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10349619297019;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.061897e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.975361e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.790290224340521;-9.9733878333772;-6.65016665510897]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88217713058974;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81636145951479;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97391323987334;6.37994517082569]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84021162478842;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79838070817225;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.670420e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.026389e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.790290224340521;-9.97242386914323;-6.65533837674166]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03641140802484;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.97059573694989;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9725658958086;6.37760232618521]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05578667514613;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01395575852997;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.819806e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.712594e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.790290224340521;-9.97244070491709;-6.65504751950077]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03371768420725;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.9679020131323;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97305160148192;6.37857731368943]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97807376741581;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.93624285079965;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.051305e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.377487e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.790290224340521;-9.97337762386633;-6.65040265951134]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88381065232848;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81799498125354;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97362609460237;6.37928705585024]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88615486814309;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84432395152692;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.090447e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.114648e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.790290224340521;-9.97165739755662;-6.65914972879035]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15904686188157;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09323119080662;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97210659100597;6.37700279093569]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1292754435676;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.08744452695143;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.240706e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.905103e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.790290224340521;-9.97359318530882;-6.64918723932138]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84932082153029;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78350515045534;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97400927055586;6.38001680169222]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82484671558458;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78301579896842;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.533745e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.430808e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.790290224340521;-9.97197335293672;-6.65766372840793]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.108494001066;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04267832999105;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97225616681313;6.37709660295033]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.10534331442165;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.06351239780548;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.898494e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.942891e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.790290224340521;-9.97292633572621;-6.65259279174331]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.9560167547476;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.89020108367266;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97341329644269;6.37910520373929]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.92020257369262;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.87837165707645;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.274986e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.281202e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.790290224340521;-9.97299998310953;-6.65243592844012]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.94423317341591;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.87841750234096;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97330358206705;6.37869483191199]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.93775687379387;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.8959259571777;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.772701e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.060507e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.790290224340521;-9.97191216989538;-6.65785056790889]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11828328768086;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.05246761660592;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97234434230292;6.37735782197954]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.09123523605483;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.04940431943866;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.527517e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.919474e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360891401341;-6.64920413223827]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84680422879537;-0.00246155353175017;-0.00729489384718771;-0.00246155353175017;-1.78098855772042;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97399110904533;6.37989425074401]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82775255727008;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78592164065391;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.507577e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.017331e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.790290224340521;-9.97168300741337;-6.65914632697317]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15494928480134;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08913361372639;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97205862250768;6.37677784193722]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13695040329415;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09511948667799;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.327086e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.391552e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.790290224340521;-9.97332626632698;-6.65059077160867]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89202785862423;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82621218754928;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97371147158091;6.37952357362956]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.87249455157644;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.83066363496028;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.494934e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.910604e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.790290224340521;-9.97252122138224;-6.65495216700216]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02083504978281;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.95501937870787;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97293111068776;6.37804690530502]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.99735229448037;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.9555213778642;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.257062e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.185009e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.790290224340521;-9.9723444626045;-6.65565549123645]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.04911645422077;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.98330078314582;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97267947052209;6.37785817210205]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03761472098751;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99578380437134;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.785776e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.897563e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.790290224340521;-9.97343375615218;-6.65021335488621]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87482948659146;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.80901381551651;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97385959475088;6.37958527901371]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.84879484438069;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80696392776453;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.985537e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.368517e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163557669958;-6.65938209945162]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16253819900809;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09672252793314;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200661330427;6.37668953000775]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14527187583939;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10344095922322;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602792e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.369102e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.790290224340521;-9.97356739258968;-6.64942318127426]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.85344765659191;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78763198551696;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97391528094762;6.37978028187313]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83988505290265;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79805413628648;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.634740e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.667839e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.790290224340521;-9.97205320257526;-6.65737381322927]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09571805889983;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.02990238782488;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97256122730172;6.3774274815212]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05653363624585;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01470271962968;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.382194e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.685813e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.790290224340521;-9.97283407079285;-6.65317818297134]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97077914408538;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.90496347301043;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97305668538072;6.37841766968997]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97726034360694;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.93542942699077;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.863139e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.330063e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.790290224340521;-9.97308535142769;-6.65210157692953]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.93057424251027;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86475857143533;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97362229694565;6.37911268152346]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88676249321742;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84493157660126;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.835447e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.878042e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.790290224340521;-9.97184463983549;-6.65831438763866]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1290880972619;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06327242618696;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97210894570501;6.3768391524276]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12889869171955;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.08706777510338;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.867820e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.651160e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362111526321;-6.64924087061946]
Robot Arm 1 Joint Angles: [-0.00246155353174371;1.84485202882709;-0.00246155353175017;-0.00729489384718771;-0.00246155353175017;-1.77903635775215;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97400979780328;6.37984931789737]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82476235599682;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78293143938065;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.826652e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.949308e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.790290224340521;-9.97172283481346;-6.65906418710046]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14857690078699;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.08276122971204;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9722526945826;6.37692384304875]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.10589887130644;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.06406795469028;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.719632e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.862916e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.790290224340521;-9.97325835030204;-6.6510482356226]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90289442261427;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83707875153933;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97341780863218;6.37894455156729]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91948062337283;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.87764970675667;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.010779e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.807320e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.790290224340521;-9.97261895757049;-6.65456238770493]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00519725966267;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93938158858773;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97329883702291;6.37851923175133]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.93851608085564;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.89668516423948;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.904401e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402499e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.790290224340521;-9.97225044218936;-6.65625251257159]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.06415972064254;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.9983440495676;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97234830691329;6.37719660684876]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.09060089839416;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04876998177799;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.461275e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.524071e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.790290224340521;-9.97348302829508;-6.65005886277073]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86694594372826;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.80113027265332;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97399010228041;6.37972419108624]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82791363965508;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78608272303892;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.224836e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.234470e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162398565347;-6.65956250931129]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16439276638491;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09857709530996;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97205693011719;6.37660795717146]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1372211857716;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09539026915544;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.392880e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.373523e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [0.790290224340521;-9.97353422314328;-6.64969793608353]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85875476801687;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79293909694193;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97371488050102;6.37936103352538]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.87194912435749;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.83011820774132;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.429224e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.691685e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [0.790290224340521;-9.97213854527345;-6.65705472210679]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08206322718905;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01624755611411;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97292603031204;6.3778711109381]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.99816515459464;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95633423797848;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.029475e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.466828e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [0.790290224340521;-9.97273909947864;-6.65377791311313]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98597455435759;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.92015888328265;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97268438024171;6.3776983438795]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0368291658481;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99499824923193;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.653746e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.157245e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [0.790290224340521;-9.97316596731255;-6.65179027663018]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91767570093292;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.85186002985798;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97385710558873;6.37941284611589]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.84919311032499;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80736219370883;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.239171e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531427e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [0.790290224340521;-9.97178494441868;-6.65873862528428]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13863936395208;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07282369287714;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200698540539;6.37652286166494]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14521233965905;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10338142304288;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.053005e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.300341e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362580030212;-6.64931684907593]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84410242260201;-0.00246155353175017;-0.00729489384718771;-0.00246155353175017;-1.77828675152706;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97391729555927;6.37961534893446]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83956271503797;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79773179842181;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.311923e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.676215e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [0.790290224340521;-9.97177190983818;-6.65893446155508]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14072489683231;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07490922575736;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97255658552119;6.37725268091795]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05727632113126;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01544540451509;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.845229e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.655811e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [0.790290224340521;-9.9731844746504;-6.65153698724123]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.9147145268773;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.84889885580236;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97306174928374;6.37825799195279]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.9764501191233;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.93461920250713;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.987061e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.909087e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [0.790290224340521;-9.9727160812081;-6.65417411005502]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98965747764425;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.92384180656931;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97361849637115;6.37893830680796]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88737058513835;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84553966852219;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.148289e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.572574e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [0.790290224340521;-9.97215967500123;-6.65683331829809]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07868247074376;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01286679966882;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97211131738015;6.37667553719081]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12851922369732;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08668830708115;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.986007e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.928974e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [0.790290224340521;-9.97352522973228;-6.6499404707643]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86019371377581;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79437804270087;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401030287432;6.37968179777711]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82468154463014;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78285062801398;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.695519e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.352703e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162276320538;-6.65969018030904]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1645883580799;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09877268700496;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97224925440744;6.37675113325486]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.10644929933172;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.06461838271555;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.623171e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.043345e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [0.790290224340521;-9.97349377876718;-6.65001097202095]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86522586819156;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79941019711661;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97342229406609;6.37878385495876]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91876295394802;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.87693203733185;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.651297e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.632629e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [0.790290224340521;-9.97222842381957;-6.65671120707347]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.06768265980878;-0.00246155353175017;-0.00729489384718762;-0.00246155353175017;-2.00186698873384;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97329409885058;6.37834364723845]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.93927418842893;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.89744327181276;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.859765e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.427507e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [0.790290224340521;-9.97264230158353;-6.65438744539502]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00146221757653;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93564654650159;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97235227575472;6.3770353953119]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.08996588376736;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04813496715119;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.824134e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.063500e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [0.790290224340521;-9.97324125050984;-6.65150515769188]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90563038936567;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83981471829072;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398907866009;6.37955410519557]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82807741890742;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78624650229125;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.527648e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.490972e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [0.790290224340521;-9.97173377048497;-6.65911976692081]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14682719334492;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.08101152226998;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97205526976724;6.37643812059776]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13748684176286;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09565592514669;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.801504e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.919376e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362295349986;-6.64943225394575]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8445579109639;-0.00246155353175017;-0.00729489384718771;-0.00246155353175017;-1.77874223988895;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97371826095677;6.37919844612567]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.87140825143864;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82957733482248;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.430225e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.225896e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [0.790290224340521;-9.97182967743396;-6.65875983755537]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.131482081507;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06566641043206;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97292096749902;6.37769534816693]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.99897520467896;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.9571442880628;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.996816e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.755917e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [0.790290224340521;-9.97310510886695;-6.65205455529231]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.92741305222857;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86159738115362;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9726892809341;6.37753849890463]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03604505506588;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99421413844972;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.274496e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.445271e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [0.790290224340521;-9.97281163762226;-6.65379222392789]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97436845137886;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.90855278030392;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97385460585961;6.37924039901722]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.84959306698469;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80776215036852;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.110304e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.963009e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [0.790290224340521;-9.97207317263594;-6.65739274864021]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09252284919022;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.02670717811528;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200738410456;6.37635623191926]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1451485477917;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10331763117554;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.034332e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.899510e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [0.790290224340521;-9.97356019961818;-6.64985920478739]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85459853203201;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78878286095707;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97391928388633;6.37945037231571]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83924458270865;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79741366609248;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.324525e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.660872e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163192978293;-6.65976492365502]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1631217056728;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09730603459786;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9725519704941;6.37707792436388]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05801472546565;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01618380884948;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.454520e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.477144e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [0.790290224340521;-9.97344620093542;-6.65036153668118]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87283832127348;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80702265019854;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9730667930891;6.3780982803274]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97564311026602;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.93381219364986;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.141476e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.337006e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [0.790290224340521;-9.97232183880593;-6.65634825087452]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.05273626199261;-0.00246155353175017;-0.00729489384718762;-0.00246155353175017;-1.98692059091766;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97361469310135;6.37876393205339]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88797910830555;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84614819168938;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.168119e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.913368e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [0.790290224340521;-9.97254461810098;-6.65500194067142]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01709157478353;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.95127590370858;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97211370571022;6.37651194473177]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12813709088565;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08630617426949;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.751333e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.784565e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [0.790290224340521;-9.97331070457088;-6.65124894259436]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89451773960077;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82870206852583;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401078597106;6.37951424168511]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82460424915279;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78277333253662;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.980741e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.204576e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [0.790290224340521;-9.97169170850645;-6.65945478138227]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15355710990954;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.0877414388346;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97224584618718;6.37657847336889]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.10699461457233;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.06516369795617;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.931713e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.844763e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361257711201;-6.64958715535148]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84621813301986;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78040246194492;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97342675276808;6.37862311395985]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91804956162976;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.8762186450136;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.037499e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.193600e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [0.790290224340521;-9.97189548289754;-6.65854350541426]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12095320733494;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.05513753626;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97328936775057;6.3781680786589]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.9400311644308;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.89820024781463;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.788030e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.945869e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [0.790290224340521;-9.97302080691691;-6.65259803835494]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.94090136423565;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.8750856931607;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97235624853216;6.37687418692115]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08933023937671;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04749932276055;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.775669e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.412687e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [0.790290224340521;-9.97290472806937;-6.6534213667202]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.95947397984209;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.89365830876715;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398803839626;6.37938399344238]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82824386112013;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78641294450397;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.503414e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.873396e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [0.790290224340521;-9.97199191060132;-6.65792583051692]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1055247747301;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03970910365515;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9720536412436;6.37626833185797]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13774740554655;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.09591648893038;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.410965e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.337305e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358781844026;-6.6498158670794]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85017952050026;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78436384942532;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97372161307013;6.37903581163734]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.87087191330043;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82904099668426;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.057083e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.583664e-97。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [0.790290224340521;-9.97165138706318;-6.65978714195694]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16000854083176;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09419286975681;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97291592238288;6.37751961715216]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.99978242326135;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95795150664519;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.172642e-97。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.111903e-97。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [0.790290224340521;-9.97339167921527;-6.65074861682393]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88156179649831;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81574612542336;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97269417238292;6.37737863685122]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0352624232552;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99343150663903;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.026112e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.616344e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [0.790290224340521;-9.97241776458635;-6.65597098993037]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03738813712514;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.9715724660502;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97385209578293;6.37906793808734]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.84999467925213;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80816376263596;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.994966e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.166213e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [0.790290224340521;-9.97244703785028;-6.65561632570443]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03270441489562;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.96688874382067;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200780910993;6.37618964031031]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14508054693263;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10324963031646;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.107498e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.334496e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [0.790290224340521;-9.97337391344531;-6.6510239583368]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88440431969208;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81858864861713;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97392124610686;6.37928535232306]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83893062742447;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7970997108083;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.275515e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.501513e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [0.790290224340521;-9.97165924536407;-6.6597411552971]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15875121269035;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09293554161541;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97254738224489;6.37690321184361]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05874884533928;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01691792872311;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.325682e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.579440e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [0.790290224340521;-9.97359469204746;-6.64978150332204]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84907974334657;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78326407227163;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97307181669783;6.37793853466783]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97483933286823;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.93300841625207;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.822938e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.145231e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [0.790290224340521;-9.97196857983976;-6.65828912135079]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.10925769657867;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04344202550372;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9736108873575;6.37858955760683]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88858802732069;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84675711070453;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.984926e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.786583e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [0.790290224340521;-9.97293220897271;-6.65316409868076]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.95507703530724;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.8892613642323;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97211611037562;6.37634837455942]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12775234442202;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08592142780586;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.337936e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.106498e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [0.790290224340521;-9.97299452146051;-6.65306586319817]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.94510703725954;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.8792913661846;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401124729509;6.37934664997423]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82453043730755;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78269952069139;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.916466e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.846094e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [0.790290224340521;-9.97191681314825;-6.65842785260007]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11754036722186;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.05172469614692;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97224246981934;6.37640586318839]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.10753483342626;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.0657039168101;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.967322e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.417360e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360800040966;-6.64981107093048]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84695040539635;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78113473432141;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97343118476392;6.37846232862006]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91734044229478;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.87550952567862;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.898931e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.412305e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [0.790290224340521;-9.97168091891246;-6.65975782508081]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15528344494777;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08946777387283;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97328464392143;6.3779925262948]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94078697709323;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.89895606047707;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.396592e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.350252e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [0.790290224340521;-9.97333046012592;-6.65117089624407]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89135685079406;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82554117971912;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97236022495311;6.3767129812326]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08869401202431;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04686309540814;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.744259e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.293945e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [0.790290224340521;-9.97251516619268;-6.65558463208923]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.0218038801123;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.95598820903736;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398698170018;6.37921385619564]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8284129324926;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78658201587643;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.454614e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.168899e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [0.790290224340521;-9.97235058209787;-6.65622537110838]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.0481373352821;-0.00246155353175017;-0.00729489384718762;-0.00246155353175017;-1.98232166420716;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97205204433108;6.376098590593]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13800291154929;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09617199493313;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.140550e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.126339e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [0.790290224340521;-9.97343053584227;-6.65083215959327]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87534473617781;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80952906510286;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97372493696406;6.37887313026892]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.87034009027261;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82850917365645;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.117318e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.543439e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163675852979;-6.65997692123826]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16234910617461;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09653343509967;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97291089509523;6.3773439180504]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00058678928445;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95875587266828;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.680489e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.433522e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [0.790290224340521;-9.97356934053835;-6.65001511491203]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85313598480456;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78732031372962;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97269905437485;6.37721875739772]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03448130454526;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.9926503879291;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.820802e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.029109e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [0.790290224340521;-9.97204813975225;-6.65800076256598]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09652811058088;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03071243950594;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9738495755773;6.37889546369406]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85039791215369;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80856699553752;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.443433e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.439877e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [0.790290224340521;-9.97284004017351;-6.65374897090974]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96982404317974;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.9040083721048;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200826013001;6.37602308637855]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14500838372076;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10317746710459;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.331643e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.142457e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [0.790290224340521;-9.97308026323931;-6.65272967871701]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.93138835265028;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86557268157534;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97392318239893;6.37912028926279]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8386208206925;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79678990407633;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.928384e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.600524e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [0.790290224340521;-9.97184873957913;-6.65889443277826]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1284321382795;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06261646720456;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97254282079536;6.37672854333798]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.05947867726455;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01764776064839;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.177426e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.175169e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362068704886;-6.64984527009436]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84492054312374;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77910487204879;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97307682001389;6.37777875483261]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97403880230001;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.93220788568384;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.049229e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.824691e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [0.790290224340521;-9.97172019366001;-6.65967853976206]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14899948533917;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08318381426422;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97360707935958;6.37841518381281]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88919730698915;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84736639037299;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.165598e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.156962e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [0.790290224340521;-9.97326285586149;-6.65162671451388]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90217353310265;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83635786202771;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97211853105829;6.37618482618527]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12736503519528;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08553411857911;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.188622e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.682830e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [0.790290224340521;-9.97261301665184;-6.65519437181156]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00614780664602;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.94033213557107;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401168704755;6.37917902299647]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82446007691407;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7826291602979;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.015647e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.305610e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [0.790290224340521;-9.97225628785015;-6.65682377552848]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.06322441491706;-0.00246155353175017;-0.00729489384718762;-0.00246155353175017;-1.99740874384211;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97223912519945;6.37623330250833]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.10806997261008;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.06623905599391;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.689703e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.841018e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [0.790290224340521;-9.97348029793942;-6.65067516013101]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86738280063229;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80156712955734;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97343559008145;6.37830149899217]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91663559149073;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.87480467487457;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.320502e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.454482e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162451160129;-6.66016067930568]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16430861473296;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09849294365801;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97327992755974;6.37781699042484]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94154159496334;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.89971067834717;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.945475e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.189375e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [0.790290224340521;-9.97353659061746;-6.65028765272975]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85837597214604;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79256030107109;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97236420472757;6.3765517778062]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08805724811081;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04622633149465;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.551173e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.958993e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [0.790290224340521;-9.97213326325938;-6.65768287406669]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08290834943952;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01709267836457;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398590878245;6.37904369382297]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82858459932958;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78675368271341;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.073775e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.461102e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [0.790290224340521;-9.97274510618874;-6.65434848660216]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98501348074195;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.919197809667;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97205047881364;6.37592889644282]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13825339433895;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09642247772279;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.922784e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.806734e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [0.790290224340521;-9.97316128113067;-6.65241638728106]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91842549003234;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.8526098189574;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9737282327624;6.37871040223052]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86981276253791;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82798184592175;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.300531e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.158669e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [0.790290224340521;-9.9717884721745;-6.65932157739772]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13807492302015;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07225925194521;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97290588576512;6.37716825101425]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0013882821033;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.95955736548713;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.275087e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.604183e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362584229683;-6.64991878130009]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84409570344769;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77828003237275;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97270392669962;6.37705886022717]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03370173258351;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99187081596735;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.442606e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.386881e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [0.790290224340521;-9.971768767714;-6.65955141294267]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14122763670068;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07541196562573;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97384704546044;6.37872297620331]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85080273085187;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.8089718142357;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.137436e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.548661e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [0.790290224340521;-9.97318925224337;-6.65211402972702]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91395011200145;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.8481344409265;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200873687366;6.37585656966533]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14493210473609;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10310118811993;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.116243e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.827475e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [0.790290224340521;-9.97271031402806;-6.65480530603398]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99058022645125;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.92476455537631;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97392509294063;6.37895518344127]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83831513402107;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7964842174049;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.213233e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.316393e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [0.790290224340521;-9.97216519054518;-6.65740625241714]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07779998371134;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01198431263639;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97253828616468;6.3765539188241]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.06020421817288;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01837330155671;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.135868e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.259937e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [0.790290224340521;-9.97352298507455;-6.65055426950807]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86055285901185;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79473718793691;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97308180294406;6.37761894068473]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97324153347273;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.93141061685657;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.766370e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.298469e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162265116621;-6.66029161230984]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16460628434741;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09879061327247;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97360326932626;6.3782408110133]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.88980691232014;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84797599570398;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.944005e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.724486e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [0.790290224340521;-9.97349654222285;-6.65059859571675]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86478371528401;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79896804420906;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97212096744175;6.37602129912337]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12697521384162;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08514429722545;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.732653e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.673312e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [0.790290224340521;-9.9722229930779;-6.65734020700069]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.06855157847648;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00273590740153;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401210542909;6.37901136110299]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82439313586785;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78256221925169;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.187267e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.989698e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [0.790290224340521;-9.97264828556758;-6.65495811460927]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00050478012805;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.9346891090531;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97223581222106;6.37606079112117]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.10860004915185;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.06676913253568;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.983143e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.752878e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [0.790290224340521;-9.97323698768707;-6.65212915491376]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90631244100926;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.84049676993432;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9734399687505;6.3781406251322]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91593500444141;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.87410408782524;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.940976e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.113245e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [0.790290224340521;-9.97173670575861;-6.65970573225012]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14635754956314;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08054187848819;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97327521886011;6.37764147132428]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94229498690385;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.90046407028768;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.963977e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.837325e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362344935446;-6.65003179873601]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84447857422685;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77866290315191;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97236818756803;6.37639057620576]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08741999363705;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04558907702088;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.509077e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.938044e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [0.790290224340521;-9.97182609054572;-6.65937910870254]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13205598362582;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06624031255088;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398481985299;6.37887350669054]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8287588280438;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78692791142764;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.192208e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.693763e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [0.790290224340521;-9.97311011525096;-6.65263038844854]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.92661203078589;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86079635971095;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97204894447438;6.37575924904654]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13849888862129;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09666797200513;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.370155e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.285971e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [0.790290224340521;-9.97280609745716;-6.6544223542749]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97525487779465;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.9094392067197;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9737315005899;6.37854762773377]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86928991013769;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82745899352153;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.502065e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.278771e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [0.790290224340521;-9.97207830684299;-6.65796761593194]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.0917013760623;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.02588570498736;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97290089451898;6.37699261619233]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00218688148423;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96035596486806;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.134456e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.042778e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [0.790290224340521;-9.97355843305972;-6.65047053202053]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85488118138439;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78906551030945;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9727087891499;6.37689894502726]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03292374053686;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99109282392069;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.378115e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.350257e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163120499365;-6.66036949461722]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16323767195704;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09742200088209;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97384450564923;6.37855047597912]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85120910064432;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80937818402815;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.646337e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.978661e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [0.790290224340521;-9.97344933465149;-6.65094720351506]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87233692670169;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80652125562675;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200923905017;6.3756900897129]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14485175649337;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10302083987721;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.196704e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.301629e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [0.790290224340521;-9.9723163285987;-6.65697774878516]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.05361789514799;-0.00246155353175017;-0.00729489384718762;-0.00246155353175017;-1.98780222407305;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97392697791001;6.3787900351649]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83801353892027;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79618262230411;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.415876e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.344725e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [0.790290224340521;-9.9725505190668;-6.65557301625541]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01614742025178;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.95033174917683;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97253377836945;6.37637933827541]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.06092546540896;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.0190945487928;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.847849e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.481269e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [0.790290224340521;-9.97330687976555;-6.65187073781154]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89512970845228;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.82931403737734;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97308676539798;6.37745909209159]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97244754084427;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.93061662422811;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.264632e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.969247e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [0.790290224340521;-9.97169403884941;-6.66004382565167]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15318425503499;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08736858396004;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97359945747495;6.37806643954769]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89041680852887;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.8485858919127;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.053102e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.700736e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361350940553;-6.65018439979076]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84606896605495;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78025329498;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9721234192111;6.37585779289036]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12658293074449;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08475201412832;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.524827e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.603589e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [0.790290224340521;-9.9718915110967;-6.65916479848798]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12158869546832;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.05577302439337;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401250263986;6.37884366464405]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82432958214373;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78249866552756;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.921329e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.748042e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [0.790290224340521;-9.97302599551241;-6.65317290590657]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.94007118895412;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.87425551787918;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97223253077583;6.37588832881683]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.10912508038764;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.06729416377148;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.847983e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.000028e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [0.790290224340521;-9.97289946145459;-6.65405018650692]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96031663820537;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.89450096713043;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97344432080292;6.37797970709939]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91523867605277;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.87340775943661;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.745307e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.049800e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [0.790290224340521;-9.97199661812066;-6.65850286296737]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.10477157163526;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03895590056031;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97327051801517;6.37746596926497]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94304712209329;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.90121620547712;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.479073e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.300773e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358651971909;-6.65042476503502]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85038731588563;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78457164481069;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97237217318949;6.37622937599901]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08678229420216;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.044951377586;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.105676e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.078356e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [0.790290224340521;-9.97165008155981;-6.66039469467252]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.16021742137117;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09440175029622;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398371512104;6.37870329516318]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82893558515409;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78710466853793;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.290356e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.058200e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [0.790290224340521;-9.97339515497834;-6.65133247629319]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88100567440604;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.8151900033311;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97204744109555;6.37558964804245]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13873942923239;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09690851261623;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.407367e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.892374e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [0.790290224340521;-9.97241224185955;-6.65660064602945]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03827177341222;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.97245610233728;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97373474057216;6.37838480699178]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8687715129749;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82694059635873;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.187076e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.405148e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [0.790290224340521;-9.97245279635162;-6.65618811330545]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03178305468126;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.96596738360631;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97289592148074;6.37681701372931]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00298256760219;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96115165098602;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.167233e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.402031e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [0.790290224340521;-9.97337053525465;-6.65164349385393]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88494483019649;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81912915912154;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97271364152141;6.37673901149022]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03214736109545;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.99031644447929;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.474279e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.907841e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [0.790290224340521;-9.97166096630753;-6.66033330410548]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15847586173628;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09266019066133;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97384195635972;6.37837796338366]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85161698696646;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.80978607035029;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.418456e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.354973e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [0.790290224340521;-9.9735960422711;-6.65037654170737]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84886370756524;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.7830480364903;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97200976636925;6.37552364606447]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14476738544029;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10293646882412;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.130697e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.992491e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [0.790290224340521;-9.97196428569311;-6.65891212416554]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.10994476004312;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04412908896818;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97392883748509;6.37862484474011]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83771600690652;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79588509029036;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.077733e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.529157e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [0.790290224340521;-9.9729375302204;-6.65373825903719]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.95422563567674;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88840996460179;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9725292974237;6.37620480166169]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06164241672812;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.01981150011196;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.405215e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.731409e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [0.790290224340521;-9.97298956786503;-6.65369316192948]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.94589961253564;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88008394146069;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97309170728811;6.37729920892493]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97165683842331;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92982592180714;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.069558e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.744323e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [0.790290224340521;-9.97192105513768;-6.65900724905219]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11686164891102;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.05104597783607;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97359564402178;6.37789206975282]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89102696103652;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.84919604442036;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.043906e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.766751e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [0.790290224340521;-9.9736071571774;-6.65041759417368]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84708532255659;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78126965148164;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97212588605307;6.3756943070055]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12618823603028;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08435731941411;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.313179e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.455464e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [0.790290224340521;-9.97167907091307;-6.66036817121147]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.15557912484938;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08976345377444;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401287887954;6.37867593396905]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82426938379441;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78243846717825;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.470569e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.260865e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [0.790290224340521;-9.97333424695263;-6.65175311242706]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89075095851906;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82493528744411;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97222928075354;6.37571591538282]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.10964508395417;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.067814167338;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.012034e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.651453e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [0.790290224340521;-9.97250969451209;-6.65621412207692]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02267934900532;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.95686367793037;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97344864627252;6.37781874495607]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91454660091791;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.87271568430174;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.663706e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.290920e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [0.790290224340521;-9.97235614063393;-6.65679816585489]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.04724796951163;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.98143229843668;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97326582521559;6.37729048451535]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94379797002624;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90196705341007;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.518132e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.963812e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [0.790290224340521;-9.97342760752248;-6.65144940530202]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87581326734286;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80999759626791;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97237616130947;6.37606817675751]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.0861441950053;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04431327838913;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.259186e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.861060e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163787340395;-6.6605721610083]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16217072630833;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09635505523338;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398259479521;6.37853305960426]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82911483728791;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78728392067174;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.763655e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.397362e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [0.790290224340521;-9.9735710889821;-6.65060804916277]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85285623380343;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78704056272848;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97204596845866;6.37542009306804]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13897505113544;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09714413451928;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.019056e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.700092e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [0.790290224340521;-9.97204358756754;-6.65862515332392]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.0972564601332;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03144078905825;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97373795283563;6.37822194021912]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86825755081973;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82642663420356;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.509357e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.095938e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [0.790290224340521;-9.97284544206969;-6.65432269433115]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96895973978911;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.90314406871417;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97289096677176;6.37664144376593]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0037753210396;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96194440442344;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.827582e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.460728e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [0.790290224340521;-9.97307565496867;-6.65335528108298]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.93212567595318;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86631000487824;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97271848361279;6.37657905931286]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0313726264743;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98954170985814;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.027495e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.331610e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [0.790290224340521;-9.97185248417552;-6.65947635666259]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12783300285703;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06201733178209;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97383939780707;6.3782054387772]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85202635539084;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81019543877467;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.510248e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.164264e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362028533107;-6.65044948325456]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84498481796824;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77916914689329;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201031854106;6.37535723826426]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14467903795171;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.10284812133555;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.285038e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.759047e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [0.790290224340521;-9.97171784688176;-6.6602914631816]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14937496985833;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08355929878338;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97393067184386;6.37845961247332]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83742250950332;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79559159288716;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.640934e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.991519e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [0.790290224340521;-9.97326691979403;-6.65220746687948]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90152330389493;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83570763281998;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97252484333894;6.37603030894916]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06235507029083;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02052415367467;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.380063e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.059318e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [0.790290224340521;-9.97260765522919;-6.65582339178515]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00700563426965;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.94118996319471;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97309662852967;6.37713929106084]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97086943977425;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92903852315808;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.133509e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.261764e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [0.790290224340521;-9.97226159193067;-6.65739785670864]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.06237576203362;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.99656009095868;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97359182918156;6.37771770196292]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89163733547184;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.84980641885568;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.933134e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.547105e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [0.790290224340521;-9.97347781816534;-6.65129010998568]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86777956448625;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.8019638934113;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97212836765596;6.37553084099061]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12579117956749;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08396026295132;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.423773e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.570506e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162503125177;-6.66075895873106]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.1642254706578;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09840979958285;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9740132343473;6.37850816942647]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82421250895292;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78238159233675;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.053070e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.785494e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [0.790290224340521;-9.97353871617954;-6.65087859316338]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8580358822141;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79222021113915;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97222606204211;6.37554355060426]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11016007778452;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.06832916116835;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.681286e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.666764e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [0.790290224340521;-9.97212851807377;-6.65830832629456]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08366757913674;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.0178519080618;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97345294519499;6.3776577387677]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91385877332285;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.87202785670669;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.121607e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.039435e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [0.790290224340521;-9.97275053498519;-6.65492205156838]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98414487330918;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.91832920223424;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97326114065005;6.37711501734044]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94454750051367;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.9027165838975;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.207842e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.734489e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [0.790290224340521;-9.97315704344848;-6.65304015284309]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91910351918286;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85328784810792;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97238015164798;6.37590697805671]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.08550574084506;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.0436748242289;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.366069e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.493139e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [0.790290224340521;-9.97179169480148;-6.6599061552758]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13755930270286;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07174363162791;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398145908338;6.37836280037575]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82929655118099;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78746563456482;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.514440e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.385560e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362586683087;-6.65052075736527]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84409177800101;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77827610692606;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97204452634442;6.37525058376006]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13920578941474;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09737487279857;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.581200e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.237360e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [0.790290224340521;-9.97176597063228;-6.6601666733509]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14167516977495;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.0758594987;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97374113750755;6.37805902763173]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86774800331331;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82591708669714;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.449380e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.994186e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [0.790290224340521;-9.97319355625109;-6.65269351342024]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91326147076626;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.84744579969132;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97288603051086;6.37646590643901]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00456512278406;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96273420616789;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.255677e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.977429e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [0.790290224340521;-9.97270511687721;-6.65543357678856]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99141177058609;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.92559609951115;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97272331522566;6.37641908819653]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.03059956841597;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98876865179981;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.282119e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.098323e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [0.790290224340521;-9.97217018967462;-6.65798187858607]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07700012300124;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01118445192629;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97383683020558;6.37803290251812]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85243717162806;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.8106062550119;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.295799e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.804608e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [0.790290224340521;-9.97352094869437;-6.65116693798271]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86087867984174;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79506300876679;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201089527622;6.37519086585748]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14458676032595;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.10275584370979;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.843101e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.270406e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716225935769;-6.66089284425126]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16461549863645;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09879982756151;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97393248116423;6.37829433867091]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83713301824381;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79530210162764;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.284151e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.196826e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [0.790290224340521;-9.9734990221654;-6.65118766207881]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86438692447756;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79857125340262;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97252041612414;6.37585586010044]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06306342465861;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02123250804244;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.819582e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.390560e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [0.790290224340521;-9.97221811962142;-6.65796639462927]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.06933133151364;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00351566043869;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97310152904062;6.37697933837966]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.97008535802219;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92825444140602;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.542161e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.801610e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [0.790290224340521;-9.97265368660974;-6.65553180352342]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99964061338232;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93382494230738;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97358801316781;6.37754333650967]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89224789767244;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85041698105627;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.151795e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.620095e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [0.790290224340521;-9.97323313913228;-6.65275097682719]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.9069282097761;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.84111253870115;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97213086370973;6.3753673943702]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12539181096512;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08356089434895;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.383200e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.138681e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [0.790290224340521;-9.97173938928397;-6.66029305310484]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14592818550613;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08011251443119;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9740135692418;6.37834037136389]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82415892583396;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.7823280092178;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.139626e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.654844e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362388380594;-6.65063161790888]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84440906199023;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77859339091529;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97222287452761;6.37537123426393]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11067008010324;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.06883916348708;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.592795e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.608802e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [0.790290224340521;-9.9718228956024;-6.65999644547975]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13256717455711;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06675150348217;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97345721760793;6.37749668860273]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91317518725199;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.87134427063583;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.401162e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.716674e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [0.790290224340521;-9.97311461926562;-6.6532088139184]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.92589138844197;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86007571736702;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97325646450524;6.37693956800189]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94529568368274;0.00256857627675973;0.00650161998608438;0.00256857627679669;1.90346476706657;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.473736e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.253886e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [0.790290224340521;-9.97280111271608;-6.65504962482442]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97605243636804;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.9102367652931;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97238414392752;6.37574577947594]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.08486697611868;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04303605950252;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.918095e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.876219e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [0.790290224340521;-9.97208295538648;-6.65854502063042]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09095760910461;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.02514193802967;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97398030819278;6.37819251783821]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82948069367648;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78764977706031;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.241473e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.918339e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [0.790290224340521;-9.97355683180644;-6.65108095073332]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85513738190954;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78932171083459;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97204311453283;6.37508111975461]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13943167926919;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09760076265302;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.436379e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.670771e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163059482493;-6.66097355840308]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16333529895178;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09751962787684;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97374429471594;6.37789606944692]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86724284997112;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82541193335495;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.849945e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.276031e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [0.790290224340521;-9.97345214433039;-6.65153452613083]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8718873780776;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80607170700266;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97288111281435;6.37629040188151]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00535195422594;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96352103760977;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.255807e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.372264e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [0.790290224340521;-9.97231139025207;-6.65760435128345]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.05440803060983;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.98859235953488;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97272813616454;6.37625909784706]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02982821819441;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98799730157825;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.685552e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.792998e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [0.790290224340521;-9.9725558377339;-6.65614711066647]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01529643351671;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.94948076244177;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9738342537687;6.3778603549629]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85284940152941;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81101848491324;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.950322e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.276154e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [0.790290224340521;-9.97330343263221;-6.65249054073814]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89568124978714;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.82986557871219;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201149628586;6.3750245283904]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14449059878318;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.10265968216701;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.568932e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.850015e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [0.790290224340521;-9.97169617360689;-6.66063394084613]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15284269383837;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08702702276342;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97393426562404;6.37812902363923]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83684750467516;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.795016588059;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.184828e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.461821e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361433647044;-6.65078214887904]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84593663567054;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78012096459559;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97251601578581;6.37568145507468]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06376747879125;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02193656217508;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.004284e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.321215e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [0.790290224340521;-9.97188797386294;-6.65978393476698]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12215465286944;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.05633898179449;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97310640874165;6.37681935076598]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96930460585691;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92747368924074;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.064158e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.667756e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [0.790290224340521;-9.97303065661664;-6.65375049778182]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.9393254122781;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.87350974120316;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97358419619273;6.37736897372217]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89285861368408;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.85102769706792;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.926598e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.400115e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [0.790290224340521;-9.97289473090022;-6.65467623668621]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96107352690582;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.89525785583087;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97213337390596;6.37520396667139]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12499017956759;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08315926295142;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.002536e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.245197e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [0.790290224340521;-9.97200087602574;-6.65908225117628]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1040903068226;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03827463574766;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401388376119;6.37817254012797]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82410860273137;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.7822776861152;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.551994e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.218327e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358534284803;-6.651032979689]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85057561525499;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78475994418004;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9722197180944;6.37519896614232]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11117510941802;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.06934419280185;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.150971e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.473698e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [0.790290224340521;-9.97164894961038;-6.66100143876135]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16039853328001;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09458286220507;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9734614635508;6.37733559453263]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91249583639276;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.8706649197766;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.415468e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.140547e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [0.790290224340521;-9.97339826757767;-6.65191819719224]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88050765851311;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81469198743816;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9732517969659;6.37676413675794]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94604248997744;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90421157336127;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.816080e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.750024e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [0.790290224340521;-9.97240729963258;-6.65722735346922]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03906252972775;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.9732468586528;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9723881378731;6.37558458059836]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08422794482568;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04239702820951;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.397378e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.769214e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [0.790290224340521;-9.97245797905441;-6.65676288887135]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03095382223543;-0.00246155353175018;-0.00729489384718763;-0.00246155353175018;-1.96513815116048;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97397914232995;6.37802221235075]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82966723172986;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78783631511369;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.230732e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.333887e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [0.790290224340521;-9.97336749619384;-6.65226123125374]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88543107992561;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.81961540885067;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97204173280319;6.3749117006871]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13965275601172;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09782183939555;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.687767e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.599064e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [0.790290224340521;-9.97166254999808;-6.66092622793291]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15822247124784;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09240680017289;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97374742458957;6.37773306588329]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86674207018994;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82491115357377;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.519610e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.037351e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [0.790290224340521;-9.97359724367502;-6.65097231399778]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84867148293657;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78285581186163;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97287621379602;6.37611493022251]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00613579715787;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.9643048805417;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.458239e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.325136e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [0.790290224340521;-9.97196046399152;-6.65953277110375]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11055623229797;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04474056122303;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97273294623692;6.37609908797479]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02905860661472;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98722768999855;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.177389e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.279256e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [0.790290224340521;-9.97294230299887;-6.65431525420629]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.95346199112141;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88764632004646;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97383166870899;6.37768779646611]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85326301108309;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81143209446692;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.477112e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.807295e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [0.790290224340521;-9.97298512663701;-6.65431780399263]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.94661020901952;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88079453794457;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201212128166;6.3748582254104]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14439059945594;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.10255968283977;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.232590e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.694033e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [0.790290224340521;-9.9719248884982;-6.65958879444572]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1162483112293;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.05043264015435;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97393602540104;6.37796366768451]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83656594035567;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7947350237395;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.094510e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.684930e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360639200815;-6.65102366192172]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84720774963644;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78139207856149;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97251164232802;6.37550709382758]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06446723203879;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02263631542262;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.751373e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.873802e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [0.790290224340521;-9.97167745351434;-6.66097741617286]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15583790864676;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09002223757181;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97311126755614;6.37665932810854]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96852719553889;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92669627892272;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.549306e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.226791e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [0.790290224340521;-9.97333763325607;-6.65233738628297]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8902091499694;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.82439347889445;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97358037846723;6.37719461392693]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89346944976481;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85163853314864;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.299651e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.380398e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [0.790290224340521;-9.97250480607517;-6.65684063997108]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02346149891323;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.95764582783828;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97213589793789;6.37504055742396]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12458633445908;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08275541784291;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.884844e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.475395e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [0.790290224340521;-9.97236113582204;-6.65737388728533]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.04644873941503;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.98063306834009;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9740141781031;6.37800467606441]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82406150802609;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78223059140992;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.920021e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.591331e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [0.790290224340521;-9.97342497822238;-6.6520650563126]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87623395536048;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81041828428554;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97221659262501;6.37502674601766]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11167518452032;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.06984426790416;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.411911e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.585992e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163891088969;-6.66116787176425]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16200472859101;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09618905751606;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97346568306487;6.37717445663176]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91182071414251;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86998979752634;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.852659e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.715470e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [0.790290224340521;-9.97357264539209;-6.65120194470859]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85260720820566;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78679153713071;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97324713821477;6.37658872386344]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94678789015803;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90495697354187;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.230330e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.637003e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [0.790290224340521;-9.97203954055878;-6.65924701455609]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09790398153602;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03208831046107;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97239213321225;6.37542338101103]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08358869056204;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04175777394587;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.580320e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.075772e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [0.790290224340521;-9.97285027912092;-6.65489933923948]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96818581159375;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.9023701405188;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97397796170075;6.37785188427106]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8298561324015;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78802521578533;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.352268e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.097449e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [0.790290224340521;-9.97307153150461;-6.65397836027651]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.93278543020236;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86696975912742;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97204038093416;6.37474232619236]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.13986905505635;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09803813844018;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.133202e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.191297e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [0.790290224340521;-9.97185586563515;-6.66006019966711]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12729196931738;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06147629824243;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97375052725797;6.37757001716074]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86624564324689;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82441472663072;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.868040e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.728325e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361991768106;-6.65105347055466]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84504364197026;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77922797089531;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9728713335672;6.37593949158733]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00691663376988;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96508571715372;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.534763e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.915668e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [0.790290224340521;-9.97171578523351;-6.66090300495314]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14970483357956;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08388916250461;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97273774525313;6.37593905829449]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02829076402044;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98645984740427;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.014762e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.859638e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [0.790290224340521;-9.9732705480254;-6.6527904616077]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90094278687606;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83512711580111;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97382907523812;6.37751522738042]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85367796642213;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81184704980596;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.586143e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.642920e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [0.790290224340521;-9.97260287403381;-6.65644944552396]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00777062553175;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.94195495445681;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201276997579;6.37469195646593]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14428680839448;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.10245589177831;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.258320e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.240737e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [0.790290224340521;-9.97226635106199;-6.65797477292327]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.0616143010216;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.99579862994665;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97393776067285;6.37779827111288]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83628829686527;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7944573802491;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.192589e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.057221e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [0.790290224340521;-9.97347559606518;-6.651903676099]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8681351005118;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80231942943685;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97250729575237;6.37533277631135]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.06516268414325;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02333176752709;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.535235e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.306647e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162553425389;-6.66135740024985]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16414499031771;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09832931924277;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97311610541012;6.37649927030025]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96775313890236;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.9259222222862;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.115015e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.641855e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [0.790290224340521;-9.97354060704739;-6.65147071939694]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85773334335767;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79191767228272;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97357656020088;6.37702025744787]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89408037238135;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85224945576518;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.820136e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.544288e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [0.790290224340521;-9.97212430531535;-6.65893110253503]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08434162048449;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01852594940954;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97213843550043;6.37487716616041]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12418032445239;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08234940783623;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.173243e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.708407e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [0.790290224340521;-9.97275538758015;-6.65549859880465]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.9833684581163;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.91755278704135;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401445246466;6.377836779518]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82401761017648;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78218669356032;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.509767e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.566213e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [0.790290224340521;-9.97315325962044;-6.65366154699605]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91970893167091;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85389326059596;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97221349800031;6.37485457366604]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1121703244713;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.07033940785514;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.436082e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.241745e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 400:
Mobile Platform 1 Pose: [0.790290224340521;-9.9717946038033;-6.6604924018978]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13709386241316;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07127819133821;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97346987619319;6.37701327497739]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91114981361127;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86931889699511;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.772388e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.410139e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 401:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362588133578;-6.65112273835453]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84408945721652;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77827378614158;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97324248843263;6.37641332956992]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94753185530159;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90570093868542;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.728150e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.209430e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 402:
Mobile Platform 1 Pose: [0.790290224340521;-9.97176351008787;-6.66078028673195]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14206885688153;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.07625318580658;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97239612967496;6.37526218030483]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08294925652892;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04111833991275;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.782444e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.686543e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 403:
Mobile Platform 1 Pose: [0.790290224340521;-9.97319739201349;-6.65327541029048]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91264774878258;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.84683207770763;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97397676651034;6.37768153395537]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83004736286794;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78821644625177;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.406154e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.787552e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 404:
Mobile Platform 1 Pose: [0.790290224340521;-9.9727004914015;-6.65605891549571]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99215184670165;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.9263361756267;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203905870374;6.37457299590464]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14008061192399;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09824969530782;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.355166e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.460742e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 405:
Mobile Platform 1 Pose: [0.790290224340521;-9.9721746681042;-6.6585602182978]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07628357426854;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01046790319359;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97375360285132;6.37740692350036]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86575354831046;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82392263169429;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.466507e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.155374e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 406:
Mobile Platform 1 Pose: [0.790290224340521;-9.97351912769148;-6.65177843971623]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86117004030337;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79535436922842;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97286647223669;6.37576408609739]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00769444665123;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96586353003506;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.926169e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.344133e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 407:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716225802873;-6.66149392785752]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16461762497196;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09880195389701;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97274253302645;6.37577900852539]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02752472029052;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98569380367435;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.644659e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.684172e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 408:
Mobile Platform 1 Pose: [0.790290224340521;-9.973501225525;-6.6517781346427]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86403438694024;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79821871586529;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97382647356691;6.37734264805657]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85409423381617;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.8122633172;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.912485e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.202911e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 409:
Mobile Platform 1 Pose: [0.790290224340521;-9.97221380011079;-6.65858978837072]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07002245321481;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00420678213986;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201344208107;6.37452572110664]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14417927155154;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10234835493537;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.926564e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.714629e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 410:
Mobile Platform 1 Pose: [0.790290224340521;-9.97265850547232;-6.65610850782251]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.9988695953698;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93305392429485;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97393947161703;6.37763283423037]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83601454579707;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7941836291809;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.330641e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.398247e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 411:
Mobile Platform 1 Pose: [0.790290224340521;-9.9732297105622;-6.65337059505798]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90747678098839;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.84166110991344;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9725029760581;6.37515850247493]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.06585383522662;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02402291861045;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.856425e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.941611e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 412:
Mobile Platform 1 Pose: [0.790290224340521;-9.97174181217795;-6.66088177446381]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14554052246789;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.07972485139294;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97312092223225;6.37633917723811]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96698244736179;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92515153074562;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.329470e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.594778e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 413:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362426412888;-6.6512316732922]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84434821031944;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77853253924449;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97357274160192;6.37684590460634]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89469134821503;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85286043159886;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.681670e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.055371e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 414:
Mobile Platform 1 Pose: [0.790290224340521;-9.97182008491079;-6.66061188782947]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13301688521455;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.0672012141396;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97214098629015;6.37471379241587]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12377219809808;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08194128148191;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.624658e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.050520e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 415:
Mobile Platform 1 Pose: [0.790290224340521;-9.9731186256033;-6.65378980706375]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.9252503744129;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85943470333795;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401470704244;6.37766885083251]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82397687773159;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78214596111542;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.585860e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.177867e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 416:
Mobile Platform 1 Pose: [0.790290224340521;-9.97279668586705;-6.6556740244737]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97676073221221;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.91094506113727;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97221043409947;6.37468244886133]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11266054860687;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.0708296319907;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.359837e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.306878e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 417:
Mobile Platform 1 Pose: [0.790290224340521;-9.97208711314059;-6.65912498852052]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09029236844632;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.02447669737137;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97347404298058;6.37685204964957]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.91048312763018;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86865221101401;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.355882e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.002118e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 418:
Mobile Platform 1 Pose: [0.790290224340521;-9.9735554029209;-6.65169042446581]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85536600359739;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78955033252244;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97323784779825;6.37623795412547]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94827435680177;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.9064434401856;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.499685e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.547277e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 419:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163008944011;-6.66157716523406]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16341616052285;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09760048944791;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97240012699374;6.3751009780745]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08230968552402;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.04047876890786;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.066036e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.683007e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 420:
Mobile Platform 1 Pose: [0.790290224340521;-9.97345463657455;-6.65212346980464]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87148861901206;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80567294793711;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9739755569632;6.37751116175847]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83024089041063;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78840997379446;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.319072e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.274510e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 421:
Mobile Platform 1 Pose: [0.790290224340521;-9.97230702147195;-6.65822807150914]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.05510703542851;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.98929136435357;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203776588935;6.37440370945772]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14028746222589;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.09845654560972;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.978847e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.503290e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 422:
Mobile Platform 1 Pose: [0.790290224340521;-9.97256057390854;-6.65672422450258]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01453864557348;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.94872297449854;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97375665150053;6.37724378512446]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86526576443678;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82343484782061;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.059114e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.825882e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 423:
Mobile Platform 1 Pose: [0.790290224340521;-9.97330036915495;-6.65310832142447]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8961714061478;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83035573507285;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97286162991086;6.37558871387044]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00846921878378;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96663830216761;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.895166e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.197014e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 424:
Mobile Platform 1 Pose: [0.790290224340521;-9.97169810362992;-6.6612251733367]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15253389015367;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08671821907872;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97274730937296;6.37561893839111]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02676050484775;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98492958823158;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.327699e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.229634e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 425:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361506540727;-6.65138036530275]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84582000577782;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78000433470288;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97382386390526;6.37717005884339]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85451177968093;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.81268086306476;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.059348e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.475472e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 426:
Mobile Platform 1 Pose: [0.790290224340521;-9.97188486437354;-6.66040094988088]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12265217117412;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.05683650009917;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201413731082;6.37435951888333]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1440680347901;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10223711817394;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.217221e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.632250e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 427:
Mobile Platform 1 Pose: [0.790290224340521;-9.97303479421628;-6.65433079302508]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.9386633963354;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.87284772526045;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97394115841095;6.37746735734278]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83574465877009;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79391374215392;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.012008e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.395407e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 428:
Mobile Platform 1 Pose: [0.790290224340521;-9.97289053959659;-6.65529950235666]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96174413548551;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.89592846441056;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97249868324205;6.37498427226381]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.06654068579421;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02470976917804;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.276069e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.702056e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 429:
Mobile Platform 1 Pose: [0.790290224340521;-9.97200467843791;-6.65966402477349]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.10348192087544;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03766624980049;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97312571795379;6.37617904882315]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96621513191534;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92438421529917;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.630085e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.193452e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 430:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358429481725;-6.65164047479668]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85074330017999;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78492762910504;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97356892287727;6.37667155572108]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89530234415841;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85347142754224;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.200407e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.223607e-181。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 431:
Mobile Platform 1 Pose: [0.790290224340521;-9.97164798179637;-6.66160742241694]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.1605533835208;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09473771244586;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97214355000531;6.37455043572823]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12336200367285;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08153108705668;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.548440e-181。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.387303e-181。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 432:
Mobile Platform 1 Pose: [0.790290224340521;-9.97340102324252;-6.65250574677883]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88006675213761;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81425108106267;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401494203251;6.3775008903508]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82393927932081;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78210836270464;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.254321e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.406617e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 433:
Mobile Platform 1 Pose: [0.790290224340521;-9.9724029366229;-6.65785112026365]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.03976061127636;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.97394494020141;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97220740079999;6.3745103713754]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11314587652319;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.07131495990702;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.627276e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.510169e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 434:
Mobile Platform 1 Pose: [0.790290224340521;-9.9724625848197;-6.65734065784805]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.03021689978804;-0.00246155353175018;-0.00729489384718763;-0.00246155353175018;-1.9644012287131;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97347818347355;6.37669078073119]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90982064875346;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86798973213729;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.298233e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.090246e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 435:
Mobile Platform 1 Pose: [0.790290224340521;-9.97336480242959;-6.6528771394438]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88586208220497;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.82004641113002;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97323321648846;6.3760625977749]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.94901536636839;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90718444975222;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.917101e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.544533e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 436:
Mobile Platform 1 Pose: [0.790290224340521;-9.97166398714077;-6.66151997394114]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15799252841668;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09217685734173;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97240412490358;6.37493977391865]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08167001994909;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.03983910333292;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.629800e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.683930e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 437:
Mobile Platform 1 Pose: [0.790290224340521;-9.97359830316721;-6.65156878385736]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8485019641867;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78268629311175;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9739743332631;6.37734076803371]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83043668242551;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78860576580934;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.806874e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.827853e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 438:
Mobile Platform 1 Pose: [0.790290224340521;-9.97195710882709;-6.6601510932478]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11109305860638;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04527738753144;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203650226784;6.37423446648484]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14048964166784;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09865872505167;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.284411e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.147621e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 439:
Mobile Platform 1 Pose: [0.790290224340521;-9.97294653053738;-6.6548950671705]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.95278558495908;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88696991388413;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97375967333714;6.37708060225646]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86478227057971;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82295135396354;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.553942e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.479748e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 440:
Mobile Platform 1 Pose: [0.790290224340521;-9.97298120158556;-6.65493977119115]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.94723821725126;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88142254617631;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97285680669362;6.37541337502041]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.00924093354354;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96741001692737;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.406222e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.857318e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 441:
Mobile Platform 1 Pose: [0.790290224340521;-9.97192830669505;-6.66017252176406]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11570139973165;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.04988572865671;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97275207411165;6.37545884761963]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02599814665799;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98416723004182;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.148803e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.633361e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 442:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360571179221;-6.65162923831104]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84731658418607;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78150091311112;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97382124646218;6.3769974600878]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8549305705735;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81309965395733;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.214601e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.492575e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 443:
Mobile Platform 1 Pose: [0.790290224340521;-9.97167605781142;-6.66158560564996]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15606122111364;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.0902455500387;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201485537906;6.37419334934801]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14395314387252;0.00256857627675975;0.00650161998608431;0.00256857627679671;2.10212222725635;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.633116e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.212543e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 444:
Mobile Platform 1 Pose: [0.790290224340521;-9.97334062484243;-6.65292368731129]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88973049615143;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.82391482507649;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97394282123186;6.37730184075581]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8354786074244;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79364769080823;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.608636e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.925358e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 445:
Mobile Platform 1 Pose: [0.790290224340521;-9.97250050056109;-6.65746418888622]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02415038116525;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.9583347100903;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97249441729873;6.3748100856203]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.06722323672537;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02539232010921;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.091076e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.228933e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 446:
Mobile Platform 1 Pose: [0.790290224340521;-9.97236556560576;-6.65795254554387]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.04573997401899;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.97992430294404;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97313049250858;6.37601888496044]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96545120314969;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92362028653352;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.347829e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.489547e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 447:
Mobile Platform 1 Pose: [0.790290224340521;-9.9734226542176;-6.65267908077164]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87660579612479;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81079012504985;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97356510423253;6.37649721110827]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89591332731769;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85408241070152;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.576335e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.472974e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 448:
Mobile Platform 1 Pose: [0.790290224340521;-9.97163986166331;-6.66176410087075]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16185260481013;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09603693373518;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97214612634587;6.37438709563807]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12294978918255;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08111887256638;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.947907e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.309741e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 449:
Mobile Platform 1 Pose: [0.790290224340521;-9.97357401646227;-6.65179676632204]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85238783697707;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78657216590212;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401515763039;6.37733289841471]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82390478365985;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78207386704368;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.455519e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.481880e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 450:
Mobile Platform 1 Pose: [0.790290224340521;-9.97203599376229;-6.65986637263574]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09847146897371;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03265579789876;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97220439797779;6.37433834097799]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11362632807572;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07179541145955;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.656712e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.897084e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 451:
Mobile Platform 1 Pose: [0.790290224340521;-9.97285455375641;-6.65547889276277]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96750186991438;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.90168619883943;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97348229772035;6.37652946830781]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90916236926557;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.8673314526494;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.907064e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.127713e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 452:
Mobile Platform 1 Pose: [0.790290224340521;-9.97306789717119;-6.65459889531536]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.93336692355046;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.86755125247551;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97322859467809;6.37588726075962]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.9497548560283;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90792393941213;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.245961e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.167253e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 453:
Mobile Platform 1 Pose: [0.790290224340521;-9.97185887687061;-6.66064599760546]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12681017164226;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06099450056731;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97240812314196;6.37477856743969]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.08103030180895;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.03919938519278;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.252729e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.508422e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 454:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361959086763;-6.65165719664911]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84509593212019;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77928026104524;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97397309561313;6.37717035313295]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83063470642174;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78880378980557;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.799117e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.650851e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 455:
Mobile Platform 1 Pose: [0.790290224340521;-9.97171400040951;-6.66151320782168]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14999040541873;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08417473434378;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203526761549;6.37406526661886]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14068718604365;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.09885626942748;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.068781e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.261552e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 456:
Mobile Platform 1 Pose: [0.790290224340521;-9.97327374588579;-6.65337567071228]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90043112921498;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83461545814003;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97376266849331;6.37691737512092]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86430304559323;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82247212897706;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.808006e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.042530e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 457:
Mobile Platform 1 Pose: [0.790290224340521;-9.97259867364235;-6.65707253154216]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00844268816549;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.94262701709054;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97285200268641;6.37523806965757]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01000957469727;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96817865808111;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.390427e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.317560e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 458:
Mobile Platform 1 Pose: [0.790290224340521;-9.97227056231405;-6.65855453878485]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.06094050069327;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.99512482961832;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97275682706431;6.37529873594334]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02523767423197;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.9834067576158;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.469173e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.580982e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 459:
Mobile Platform 1 Pose: [0.790290224340521;-9.97347363796127;-6.65251582618559]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86844839713794;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80263272606299;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97381862144582;6.37682485213479]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85535057319137;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81351965657521;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.655536e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.134784e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 460:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162601136842;-6.66195605086266]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16406865199261;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09825298091766;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201559600042;6.37402721205394]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14383464445517;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.102003727839;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.180554e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.585958e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 461:
Mobile Platform 1 Pose: [0.790290224340521;-9.97354226967569;-6.65206399745914]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8574673228303;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79165165175536;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97394446025688;6.3771362847749]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8352163634216;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79338544680543;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.968344e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.338186e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 462:
Mobile Platform 1 Pose: [0.790290224340521;-9.97212062097859;-6.65955122436603]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08493111436588;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.01911544329093;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97249017822034;6.3746359424834]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06790148926794;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02607057265177;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.228981e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.589921e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 463:
Mobile Platform 1 Pose: [0.790290224340521;-9.97275966557184;-6.65607811973237]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98268397944683;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.91686830837189;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97313524583297;6.37585868555897]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96469067124648;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.92285975463031;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.829447e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.525192e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 464:
Mobile Platform 1 Pose: [0.790290224340521;-9.97314993438458;-6.65428054647113]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.92024096940818;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85442529833324;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97356128587191;6.37632287108149]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89652426501663;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85469334840047;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.510890e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.730978e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 465:
Mobile Platform 1 Pose: [0.790290224340521;-9.97179719164758;-6.66108035536396]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1366798073282;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.07086413625325;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97214871501348;6.37422377168869]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12253560236591;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08070468574974;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.050392e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.870079e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 466:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362589244116;-6.65172468955707]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8440876803548;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77827200927986;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401535403103;6.37716487536507]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.82387335955784;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78204244294167;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.883924e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.139362e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 467:
Mobile Platform 1 Pose: [0.790290224340521;-9.97176137844859;-6.66139229251275]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1424099191664;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.07659424809146;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97220142550714;6.37416635743687]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11410192337947;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.0722710067633;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.138124e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.150195e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 468:
Mobile Platform 1 Pose: [0.790290224340521;-9.97320076433057;-6.65385969514112]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91210817804957;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.84629250697462;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97348638577085;6.37636811246769]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90850828118705;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86667736457088;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.568552e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.874523e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 469:
Mobile Platform 1 Pose: [0.790290224340521;-9.9726964389986;-6.65668131643076]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99280023116435;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.9269845600894;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97322398253999;6.37571194331773]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95049279812369;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90866188150752;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.432605e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.354888e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 470:
Mobile Platform 1 Pose: [0.790290224340521;-9.9721786220886;-6.65914129033078]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07565093676516;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00983526569022;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97241212144886;6.3746173582439]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0803905727041;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.03855965608793;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.648759e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.602030e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 471:
Mobile Platform 1 Pose: [0.790290224340521;-9.97351752838358;-6.65238874226855]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86142592956789;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79561025849294;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97397184421569;6.37699991740663]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83083493001139;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78900401339522;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.016080e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.201103e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 472:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162260225027;-6.6620949092257]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16461411089848;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09879843982353;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203406170814;6.37389610949224]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14088013121967;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.0990492146035;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.026642e-199。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.412440e-199。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 473:
Mobile Platform 1 Pose: [0.790290224340521;-9.9735031584872;-6.65236998086083]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86372511298893;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79790944191399;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97376563710184;6.37675410394346]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86382806822851;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82199715161234;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.011798e-199。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.313569e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 474:
Mobile Platform 1 Pose: [0.790290224340521;-9.97221003149663;-6.65921040499921]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07062543147966;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00480976040472;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97284721798828;6.37506279788849]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0107751263967;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96894420978053;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.342829e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.028096e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 475:
Mobile Platform 1 Pose: [0.790290224340521;-9.97266274290504;-6.65668822330007]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99819160613444;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.9323759350595;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97276156805554;6.37513860309887]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02447911563615;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98264819901998;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.520264e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.182573e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 476:
Mobile Platform 1 Pose: [0.790290224340521;-9.97322670703368;-6.6539879845263]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90795734555224;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.84214167447729;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97381598906335;6.37665223532737]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85577175438563;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81394083776946;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.036295e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.166730e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 477:
Mobile Platform 1 Pose: [0.790290224340521;-9.9717439665731;-6.66147193616064]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.14519581924525;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.0793801481703;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201635889013;6.37386110655562]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14371258210094;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.10188166548477;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.534642e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.573528e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 478:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362459679815;-6.65183193091505]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84429498323668;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77847931216174;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97394607566288;6.37697068970524]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83495789846157;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7931269818454;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.776016e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.121821e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 479:
Mobile Platform 1 Pose: [0.790290224340521;-9.97181765157497;-6.66122547153956]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.13340621894622;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06759054787127;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97248596599673;6.3744618427889]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06857544504482;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02674452842865;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.863759e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.542881e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 480:
Mobile Platform 1 Pose: [0.790290224340521;-9.9731221384807;-6.65437334574503]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.92468831402886;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85887264295392;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97313997786587;6.37569845053168]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96393354598313;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.92210262936696;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.707212e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.161659e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 481:
Mobile Platform 1 Pose: [0.790290224340521;-9.97279281904296;-6.65629554367019]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97737942406732;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.91156375299237;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97355746799834;6.3761485359517]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89713512478785;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85530420817168;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.828126e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.076037e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 482:
Mobile Platform 1 Pose: [0.790290224340521;-9.97209077561579;-6.65970754218216]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08970637241389;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.02389070133895;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97215131571156;6.37406046342619]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12211949067331;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.08028857405714;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.478223e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.305623e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 483:
Mobile Platform 1 Pose: [0.790290224340521;-9.97355415267491;-6.65229892085262]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85556604295449;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.78975037187954;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401553142892;6.37699682154178]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82384497589537;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.7820140592792;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.568438e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.230800e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 484:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716296800851;-6.66218035979746]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16348165732504;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09766598625009;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97219848326086;6.37399442051793]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11457268278458;0.00256857627675974;0.00650161998608433;0.00256857627679671;2.07274176616841;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.295347e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.438935e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 485:
Mobile Platform 1 Pose: [0.790290224340521;-9.97345681726575;-6.65271400360098]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87113970842062;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80532403734568;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97349044767655;6.37620671330174]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90785837627449;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86602745965832;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.386809e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.513718e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 486:
Mobile Platform 1 Pose: [0.790290224340521;-9.97230322014727;-6.65884892150248]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.05571524737813;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-1.98989957630318;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97321938024506;6.37553664568402]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95122916531362;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.90939824869745;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.297661e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.456770e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 487:
Mobile Platform 1 Pose: [0.790290224340521;-9.9725647274893;-6.65730435793306]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01387407265331;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.94805840157836;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97241611956669;6.37445614594134]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07975087385173;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03791995723556;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.174607e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.315768e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 488:
Mobile Platform 1 Pose: [0.790290224340521;-9.97329769462208;-6.65372405342046]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89659933140726;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83078366033231;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97397057927241;6.37682946120363]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83103732093706;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78920640432089;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.245000e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.442925e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 489:
Mobile Platform 1 Pose: [0.790290224340521;-9.97169982082599;-6.66181756413691]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15225913878283;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08644346770788;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203288432103;6.37372699473702]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14106851315727;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.0992375965411;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.482611e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.106182e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 490:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361570252098;-6.65197901593223]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84571806758434;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.7799023965094;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97376857929604;6.37659078895065]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86335731715547;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.8215264005393;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.117882e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.247137e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 491:
Mobile Platform 1 Pose: [0.790290224340521;-9.97188217651897;-6.66101587571807]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12308222790495;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.05726655683;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97284245269585;6.37488755981601]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01153757318618;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.96970665657001;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.381290e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.753123e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 492:
Mobile Platform 1 Pose: [0.790290224340521;-9.97303841189511;-6.65491377280093]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.93808456772357;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.87226889664863;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97276629691277;6.37497844882722]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02372249847925;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98189158186308;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.104764e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.418715e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 493:
Mobile Platform 1 Pose: [0.790290224340521;-9.97288689031952;-6.65591997058349]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96232801981665;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.8965123487417;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97381334952116;6.37647961000669]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85619408113614;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81436316451997;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.566946e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.352643e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 494:
Mobile Platform 1 Pose: [0.790290224340521;-9.97200802020479;-6.6602482097251]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1029472381739;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03713156709895;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201714376425;6.37369503240893]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14358700224256;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.10175608562639;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.713735e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.185744e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 495:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358338182031;-6.65224721825455]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85088937969072;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78507370861578;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97394766762668;6.37680505585189]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83470318425325;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79287226763708;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.254319e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.800728e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 496:
Mobile Platform 1 Pose: [0.790290224340521;-9.97164716974991;-6.662212688449]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16068331095536;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09486763988041;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9724817806156;6.37428778646955]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06924510602667;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.0274141894105;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.689768e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.156882e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 497:
Mobile Platform 1 Pose: [0.790290224340521;-9.97340342751186;-6.65309509593713]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87968206904261;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.81386639796766;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97314468854862;6.37553817979537]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96317983674342;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.92134892012725;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.724356e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.344276e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 498:
Mobile Platform 1 Pose: [0.790290224340521;-9.97239915162444;-6.65847195386535]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.04036621103065;-0.00246155353175017;-0.00729489384718762;-0.00246155353175017;-1.9745505399557;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97355365081333;6.37597420602733]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89774587438999;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85591495777382;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.878352e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.405837e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 499:
Mobile Platform 1 Pose: [0.790290224340521;-9.97246661270763;-6.65792142470682]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02957243771998;-0.00246155353175017;-0.00729489384718762;-0.00246155353175017;-1.96375676664504;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97215392814513;6.37389717039932]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.12170150130121;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07987058468504;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.369995e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.939959e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 500:
Mobile Platform 1 Pose: [0.790290224340521;-9.97336245940377;-6.6534911910019]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88623696633696;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.82042129526201;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401569001785;6.37682873728358]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82381960166658;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78198868505041;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.163517e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.401596e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 501:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716652695267;-6.66211458377814]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15778734666807;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09197167559312;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9721955711101;6.37382252998497]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11503862690626;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07320771029009;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.749757e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.573196e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 502:
Mobile Platform 1 Pose: [0.790290224340521;-9.97359922686649;-6.65216591910244]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84835417230209;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78253850122714;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97349448349054;6.37604527090335]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90721264603595;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86538172941978;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.178013e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.867178e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 503:
Mobile Platform 1 Pose: [0.790290224340521;-9.97195421491373;-6.66076711839015]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11155608474406;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.04574041366911;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97321478796218;6.3753613680899]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95196393057245;0.00256857627675973;0.00650161998608438;0.0025685762767967;1.91013301395628;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.399371e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.127215e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 504:
Mobile Platform 1 Pose: [0.790290224340521;-9.97295021574326;-6.65547768261301]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.95219595201891;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88638028094396;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97242011724043;6.37429493014594]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07911124605298;0.00256857627675974;0.00650161998608433;0.0025685762767967;2.03728032943681;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.636046e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.341022e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 505:
Mobile Platform 1 Pose: [0.790290224340521;-9.97297779603404;-6.65555904767277]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.94778310549446;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88196743441952;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97396930098434;6.3766589848715]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83124184702805;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78941093041188;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.591168e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.466840e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 506:
Mobile Platform 1 Pose: [0.790290224340521;-9.9719313040024;-6.66075845990555]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11522183055728;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.04940615948233;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203173522911;6.37355792198511]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14125236786381;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09942145124764;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.207071e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.072522e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 507:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360512261967;-6.65223429165207]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84741085179335;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.7815951807184;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97377149520991;6.37642743037025]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86289077093675;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82105985432058;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.703991e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.936906e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 508:
Mobile Platform 1 Pose: [0.790290224340521;-9.97167487590697;-6.66219278015027]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.1562503258247;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09043465474975;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97283770690337;6.37471235553952]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01229689998328;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.97046598336711;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.722335e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.257542e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 509:
Mobile Platform 1 Pose: [0.790290224340521;-9.97334322686518;-6.65351198843892]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8893141725114;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.82349850143645;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97277101346614;6.37481827287363]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02296784993925;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98113693332308;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.940662e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.483076e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 510:
Mobile Platform 1 Pose: [0.790290224340521;-9.97249677762111;-6.6580847719057]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02474605156287;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.95893038048793;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97381070302458;6.3763069765119]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85661752058996;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81478660397379;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.302450e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.071355e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 511:
Mobile Platform 1 Pose: [0.790290224340521;-9.97236942823477;-6.65853414925338]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.04512195337672;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.97930628230177;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201795033933;6.37352898917089]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1434579502291;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.10162703361293;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.803293e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.463653e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 512:
Mobile Platform 1 Pose: [0.790290224340521;-9.973420641036;-6.65329145063546]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87692790518127;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.81111223410632;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97394923632474;6.37663938351942]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83445219256348;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79262127594731;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.753390e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.281408e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 513:
Mobile Platform 1 Pose: [0.790290224340521;-9.97164071750521;-6.66236089008014]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16171567010777;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09589999903283;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97247762206225;6.37411377345474]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.06991047456216;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02807955794599;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.049839e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.263738e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 514:
Mobile Platform 1 Pose: [0.790290224340521;-9.97357520810687;-6.65239248287934]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85219717384223;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.78638150276728;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97314937782504;6.37537787327062]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96242955251525;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.92059863589908;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.695590e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.458812e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 515:
Mobile Platform 1 Pose: [0.790290224340521;-9.97203294273868;-6.66048325112978]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09895963275163;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03314396167669;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9735498345171;6.37579988161414]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8983564817866;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85652556517043;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.738963e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.425516e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 516:
Mobile Platform 1 Pose: [0.790290224340521;-9.97285826817204;-6.65606134327262]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96690756341505;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.9010918923401;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97215655202113;6.37373389215974]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12128168114093;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07945076452476;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.049339e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.702651e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 517:
Mobile Platform 1 Pose: [0.790290224340521;-9.97306475574397;-6.65521686789819]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.93386955190567;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.86805388083073;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401582999125;6.3766606229284]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82379720592271;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78196628930654;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.366269e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.102699e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 518:
Mobile Platform 1 Pose: [0.790290224340521;-9.97186151167752;-6.66123378182738]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12638860253795;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06057293146301;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97219268892469;6.37365068560022]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11549977657126;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07366885995509;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.666435e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.002917e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 519:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361931085765;-6.65226063038694]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84514073371711;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77932506264216;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97349849326751;6.37588378536863]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90657108172037;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.8647401651042;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.078854e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.756771e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 520:
Mobile Platform 1 Pose: [0.790290224340521;-9.97171248505765;-6.66212210961509]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15023286171669;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08441719064174;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97321020585833;6.37518611076361]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95269706718966;0.00256857627675974;0.00650161998608438;0.0025685762767967;1.91086615057349;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.126382e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.172223e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 521:
Mobile Platform 1 Pose: [0.790290224340521;-9.97327651809801;-6.65396306939484]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89998757525996;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83417190418501;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97242411421741;6.37413371047538]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0784717297365;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03664081312033;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.046913e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.525909e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 522:
Mobile Platform 1 Pose: [0.790290224340521;-9.97259505450284;-6.65769264883722]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00902175048717;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.94320607941222;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97396800955165;6.37648848875611]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83144847625744;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78961755964127;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.530195e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.118164e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 523:
Mobile Platform 1 Pose: [0.790290224340521;-9.97227422316843;-6.65913716684582]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.06035476399261;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-1.99453909291766;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97203061420673;6.37338889086785]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14143173144498;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09960081482881;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.764079e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.326207e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 524:
Mobile Platform 1 Pose: [0.790290224340521;-9.97347194941036;-6.65312653188195]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86871856528316;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80290289420822;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97377438497782;6.37626402843066]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86242840807011;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82059749145394;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.785391e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.833905e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 525:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162645446633;-6.66255495191814]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16399775632791;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09818208525297;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97283298070264;6.37453718515453]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0130530920991;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.97122217548293;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.249862e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.744739e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 526:
Mobile Platform 1 Pose: [0.790290224340521;-9.97354370975235;-6.65265839741342]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85723691056436;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79142123948941;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97277571754865;6.37465807498756]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02221519673814;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.98038428012197;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.126582e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.137912e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 527:
Mobile Platform 1 Pose: [0.790290224340521;-9.97211746147989;-6.66016871106851]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08543663415881;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.01962096308386;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97380804977815;6.37613433518026]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85704204001759;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81521112340142;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.289152e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.879195e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 528:
Mobile Platform 1 Pose: [0.790290224340521;-9.97276337042025;-6.65666060652662]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98209120370135;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.9162755326264;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201877833286;6.37336297640015]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14332547126328;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.10149455464711;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.167821e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.517710e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 529:
Mobile Platform 1 Pose: [0.790290224340521;-9.97314707187633;-6.65489713097585]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.92069897072824;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.8548832996533;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395078193349;6.37647367301239]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83420489516261;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79237397854644;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.590843e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.863132e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 530:
Mobile Platform 1 Pose: [0.790290224340521;-9.97179945174972;-6.66167004897876]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.13631819098589;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.07050251991095;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97247349031993;6.37393980367114]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07057155333222;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02874063671605;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.350061e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.395198e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 531:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362590597478;-6.65232658046581]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84408551497598;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77826984390104;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97315404564137;6.37521753088193]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96168270190162;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.91985178528545;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.049024e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.131326e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 532:
Mobile Platform 1 Pose: [0.790290224340521;-9.97175956897176;-6.66200272551483]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.1426994354585;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.07688376438355;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97354601930845;6.37562556301539]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89896691516992;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85713599855375;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.518047e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.937046e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 533:
Mobile Platform 1 Pose: [0.790290224340521;-9.9732036774487;-6.65444634568129]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.91164207914812;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.84582640807318;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97215918704806;6.37357062826173]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12086007683228;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07902916021611;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.743840e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.229808e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 534:
Mobile Platform 1 Pose: [0.790290224340521;-9.97269296084206;-6.65730077484379]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99335673621057;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.92754106513563;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401595154178;6.37649247881285]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82377775783584;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78194684121967;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.514681e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.063367e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 535:
Mobile Platform 1 Pose: [0.790290224340521;-9.9721820483967;-6.65972511088062]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07510272746896;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00928705639401;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97218983657284;6.37347888712373]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11595615286616;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07412523624999;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.843387e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.371959e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 536:
Mobile Platform 1 Pose: [0.790290224340521;-9.97351615630944;-6.65299781721005]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86164546143059;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79582979035564;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97350247706363;6.3757222567959]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90593367434094;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86410275772477;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.113400e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.981447e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 537:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162265152313;-6.66269582882045]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16460622724066;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09879055616572;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97320563409844;6.3750108739299]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95342854877132;0.00256857627675974;0.00650161998608438;0.0025685762767967;1.91159763215515;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.431274e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.373426e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 538:
Mobile Platform 1 Pose: [0.790290224340521;-9.97350482648819;-6.65296317212716]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8634582328302;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79764256175526;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97242811024753;6.37397248655121]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0778323649158;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03600144829963;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.117997e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.969593e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 539:
Mobile Platform 1 Pose: [0.790290224340521;-9.97220681104588;-6.65982825951994]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07114070360009;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00532503252514;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97396670517397;6.37631797320206]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83165717668545;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78982626006928;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.793570e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.069036e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 540:
Mobile Platform 1 Pose: [0.790290224340521;-9.97266639961996;-6.6572709459869]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99760653174667;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93179086067172;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202952102799;6.37321990101671]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14160664004271;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09977572342654;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.721284e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.066840e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 541:
Mobile Platform 1 Pose: [0.790290224340521;-9.97322413295503;-6.65460312338346]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.90836919813549;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.84255352706054;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97377724873481;6.37610058336156]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86197020695209;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.82013929033592;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.090527e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.029291e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 542:
Mobile Platform 1 Pose: [0.790290224340521;-9.97174584560431;-6.66206357295189]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.14489517425053;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.07907950317558;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97282827418318;6.37436204875331]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01380613521199;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.97197521859582;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.756393e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.624635e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 543:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362488748853;-6.65243236100788]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84424847277585;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.7784328017009;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97278040899594;6.37449785492276]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.02146456517009;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97963364855392;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.876163e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.214977e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 544:
Mobile Platform 1 Pose: [0.790290224340521;-9.97181558951484;-6.66183722815244]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.13373614856607;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.06792047749113;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97380538998542;6.37596168634703]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85746760685374;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81563669023757;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.119626e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.222283e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 545:
Mobile Platform 1 Pose: [0.790290224340521;-9.97312516162324;-6.65495941040241]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.92420461122211;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85838894014716;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97201962746293;6.37319699365642]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1431896104527;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.10135869383653;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.838553e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.160269e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 546:
Mobile Platform 1 Pose: [0.790290224340521;-9.97278951406333;-6.65691417429819]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97790822080757;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.91209254973262;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395230462902;6.37630792463474]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83396126387807;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7921303472619;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.325318e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.682859e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 547:
Mobile Platform 1 Pose: [0.790290224340521;-9.97209393893472;-6.66029270111297]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08920024138523;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.02338457031028;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97246938536961;6.37376587704202]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0712283453837;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.02939742876753;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.609087e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.090009e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 548:
Mobile Platform 1 Pose: [0.790290224340521;-9.97355308655173;-6.65290641160734]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85573662266402;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.78992095158908;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97315869194624;6.37505715255731]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96093929312207;0.00256857627675974;0.00650161998608437;0.0025685762767967;1.9191083765059;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.643095e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.205661e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 549:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162935909273;-6.66278318122672]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.1635330161034;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09771734502845;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97354220538485;6.37545125053164]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.89957714294432;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85774622632815;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.538663e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.342588e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 550:
Mobile Platform 1 Pose: [0.790290224340521;-9.9734586915592;-6.65330610040417]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8708398214681;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80502415039315;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97216183293627;6.37340737826248]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.12043673471769;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07860581810152;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.459358e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.596608e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 551:
Mobile Platform 1 Pose: [0.790290224340521;-9.97229998437597;-6.65946691207333]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.05623297078564;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.99041729971069;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9740160548617;6.37632430527278]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82376122664886;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78193031003269;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.644119e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.325127e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 552:
Mobile Platform 1 Pose: [0.790290224340521;-9.97256829844157;-6.65788751082891]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01330272028915;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.9474870492142;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97218701392145;6.37330713431416]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11640777708967;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.0745768604735;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.297210e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.268591e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 553:
Mobile Platform 1 Pose: [0.790290224340521;-9.9732954136353;-6.65433771372332]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89696428929197;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83114861821702;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97350643493661;6.37556068528622]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90530041466342;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86346949804725;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.620260e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.626548e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 554:
Mobile Platform 1 Pose: [0.790290224340521;-9.97170131814888;-6.6624111489556]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15201956712052;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.08620389604557;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97320107284554;6.37483565781047]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95415834923573;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.91232743261956;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.650566e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.089186e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 555:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361625332015;-6.65257807182467]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84562993971691;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77981426864197;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97243210508314;6.37381125799873]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07719319121947;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.0353622746033;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.180112e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.183580e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 556:
Mobile Platform 1 Pose: [0.790290224340521;-9.97187990492321;-6.6616287403259]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12344568322665;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.05763001215171;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97396538805014;6.3761474385523]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83186791649806;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79003699988189;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.557209e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.298264e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 557:
Mobile Platform 1 Pose: [0.790290224340521;-9.97304151281664;-6.65549942048403]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.93758842027813;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.87177274920318;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202845546657;6.37305095206266]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14177712986932;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.09994621325315;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.205482e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.893022e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 558:
Mobile Platform 1 Pose: [0.790290224340521;-9.97288378544971;-6.65653763029637]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96282479898736;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.89700912791241;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97378008661632;6.37593709539317]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.8615161459093;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.81968522929313;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.876286e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.837960e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 559:
Mobile Platform 1 Pose: [0.790290224340521;-9.97201089687822;-6.66083482844578]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.1024869704256;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03667129935065;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97282358743212;6.37418694642428]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01455601538234;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.97272509876617;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.803369e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.199272e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 560:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358260925546;-6.65285318208946]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.8510129900674;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78519731899245;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97278508764643;6.37433761243704]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0207159810925;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97888506447633;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.313086e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.390126e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 561:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716465061608;-6.66281727425029]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16078948521202;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09497381413707;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97380272384902;6.37578903034554]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85789418867858;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81606327206241;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.839980e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.012248e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 562:
Mobile Platform 1 Pose: [0.790290224340521;-9.9734054852271;-6.65368621921673]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87935283460425;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.8135371635293;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202049744839;6.37303104050106]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14305041277908;0.00256857627675975;0.00650161998608432;0.00256857627679671;2.10121949616291;0.00256857696797632]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.022866e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.347386e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 563:
Mobile Platform 1 Pose: [0.790290224340521;-9.97239594353295;-6.65908986103753]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.04087950566889;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.97506383459395;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395380458719;6.3761421386903]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.83372127056975;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79189035395359;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.152388e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.227698e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 564:
Mobile Platform 1 Pose: [0.790290224340521;-9.97247006195205;-6.6585051930738]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02902055861235;-0.00246155353175018;-0.00729489384718763;-0.00246155353175018;-1.9632048875374;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97246530719009;6.37359199348793]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07188085410589;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03004993748972;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.446291e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.960891e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 565:
Mobile Platform 1 Pose: [0.790290224340521;-9.97336047184069;-6.65410336213351]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88655497643047;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.82073930535552;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97316331669066;6.37489673822872]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.96019933401488;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.91836841739871;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.428367e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.395465e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 566:
Mobile Platform 1 Pose: [0.790290224340521;-9.97166639002584;-6.66271009361588]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15760806680568;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09179239573074;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97353839294245;6.37527694446099]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90018713372918;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85835621711301;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.880303e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.137677e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 567:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360002010126;-6.65276369170787]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84822725473875;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.7824115836638;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97216448939792;6.37324414172194]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1200117008533;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07818078423713;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.573174e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.547137e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 568:
Mobile Platform 1 Pose: [0.790290224340521;-9.97195177760874;-6.6613808709264]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11194605354192;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.04613038246697;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9740161401427;6.37615610264281]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82374758168944;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78191666507327;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.522891e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.067773e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 569:
Mobile Platform 1 Pose: [0.790290224340521;-9.97295336118299;-6.65606308701532]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.95169268166292;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88587701058798;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97218422083602;6.37313542692821]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11685467075824;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.07502375414207;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.723796e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.864371e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 570:
Mobile Platform 1 Pose: [0.790290224340521;-9.97297491283705;-6.65617561983897]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.94824441701267;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.88242874593772;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97351036694564;6.37539907094278]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90467129321822;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86284037660205;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.788196e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.647972e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 571:
Mobile Platform 1 Pose: [0.790290224340521;-9.97193387548342;-6.66134663378419]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.11481039359319;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.04899472251824;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97319652226064;6.37466046262358]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95488644281865;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.91305552620248;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.269625e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.998690e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 572:
Mobile Platform 1 Pose: [0.790290224340521;-9.97360462978106;-6.65283879442213]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84748970597117;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78167403489623;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97243609847891;6.373650024447]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07655424789518;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03472333127901;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.220713e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.454297e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 573:
Mobile Platform 1 Pose: [0.790290224340521;-9.9716739009208;-6.66279897495796]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15640632361326;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09059065253831;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97396405837818;6.37597688514828]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83208066401278;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79024974739662;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.817324e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.042146e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 574:
Mobile Platform 1 Pose: [0.790290224340521;-9.97334544381625;-6.65410226606686]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88895946034054;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.8231437892656;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9720274172957;6.37288204363664]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14194323720851;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.10011232059235;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.297566e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.244303e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 575:
Mobile Platform 1 Pose: [0.790290224340521;-9.97249363690284;-6.65870239195844]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.02524856648699;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.95943289541204;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97378289875823;6.37577356475675]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86106620320377;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.8192352865876;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.529016e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.384451e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 576:
Mobile Platform 1 Pose: [0.790290224340521;-9.97237272224002;-6.65911870564233]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.04459491253715;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.9787792414622;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97281892053419;6.37401187825251]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01530271905;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97347180243383;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.288287e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.480447e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 577:
Mobile Platform 1 Pose: [0.790290224340521;-9.97341894346321;-6.6539021416363]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87719951682763;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81138384575269;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97278975334128;6.37417734729252]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01996946991594;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97813855329977;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.944101e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.095504e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 578:
Mobile Platform 1 Pose: [0.790290224340521;-9.97164147129547;-6.66295827555581]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16159506366534;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09577939259039;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97380005157077;6.37561636750722]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.85832175319808;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.81649083658192;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.158935e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.611167e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 579:
Mobile Platform 1 Pose: [0.790290224340521;-9.97357622545833;-6.65298906737068]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85203439760806;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78621872653312;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202138800907;6.37286511649681]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.1429079230699;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10107700645373;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.951590e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.464646e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 580:
Mobile Platform 1 Pose: [0.790290224340521;-9.97203038359529;-6.6610976706869]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.09936909569565;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.03355342462071;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395528198383;6.37597631548254]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83348488710777;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7916539704916;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.827544e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.618375e-246。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 581:
Mobile Platform 1 Pose: [0.790290224340521;-9.97286142431002;-6.65664668048836]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96640258133757;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.90058691026262;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97246125575821;6.37341815292647]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07252908320784;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03069816659167;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.111273e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.258032e-246。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 582:
Mobile Platform 1 Pose: [0.790290224340521;-9.97306211046468;-6.65583226232602]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.93429279659301;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.86847712551806;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97316791982793;6.37473628783158]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.959462832052;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.91763191543583;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.494660e-246。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.102375e-246。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 583:
Mobile Platform 1 Pose: [0.790290224340521;-9.97186376471718;-6.66182357930679]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12602811619222;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.06021244511727;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97353458217587;6.37510264509883]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90079685638176;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85896593976559;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.111485e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.806043e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 584:
Mobile Platform 1 Pose: [0.790290224340521;-9.9736190828164;-6.65286374480739]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84517722031737;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77936154924243;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97216715614669;6.37308091820275]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.11958502105154;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.07775410443537;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.374764e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.159363e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 585:
Mobile Platform 1 Pose: [0.790290224340521;-9.97171123279144;-6.66272974318621]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15043322431165;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.0846175532367;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401620757562;6.3759878712564]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82373679242254;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78190587580637;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.187815e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.903836e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 586:
Mobile Platform 1 Pose: [0.790290224340521;-9.97327886876721;-6.65455263609959]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89961146818734;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83379579711239;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97218145718047;6.37296376472083]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11729685564633;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.07546593903016;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.304252e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.308938e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 587:
Mobile Platform 1 Pose: [0.790290224340521;-9.97259201695758;-6.65830979677302]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.00950775772758;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.94369208665263;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97351427315128;6.3752374138712]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90404630031612;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86221538369995;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.678006e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.489461e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 588:
Mobile Platform 1 Pose: [0.790290224340521;-9.97227733149444;-6.65972266772093]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.0598574318302;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-1.99404176075525;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97319198250284;6.37448528858422]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95561280406715;0.00256857627675973;0.00650161998608437;0.00256857627679669;1.91378188745098;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.078597e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.294235e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 589:
Mobile Platform 1 Pose: [0.790290224340521;-9.97347053520777;-6.6537357687205]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86894483769848;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.80312916662353;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97244009019228;6.37348878552898]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.0759155737562;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03408465714003;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.925592e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.643849e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 590:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162685652719;-6.66315413912266]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16393342658982;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09811775551488;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97396271635572;6.37580631333018]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83229538760547;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79046447098931;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.323124e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.726182e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 591:
Mobile Platform 1 Pose: [0.790290224340521;-9.97354493219561;-6.65325389337328]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85704131964322;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79122564856828;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202640628866;6.37271317536967]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14210499833609;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.10027408171992;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.295580e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.029466e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 592:
Mobile Platform 1 Pose: [0.790290224340521;-9.97211482368047;-6.660783579512]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08585868206509;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.02004301099015;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97378568529709;6.37560999168448]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.86062035698661;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.81878944037044;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.354122e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.825113e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 593:
Mobile Platform 1 Pose: [0.790290224340521;-9.97276650342594;-6.65724605222517]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.98158992279075;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.9157742517158;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97281427357198;6.37383684431972]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01604623300351;0.00256857627675974;0.00650161998608436;0.0025685762767967;1.97421531638734;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.525878e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.504746e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 594:
Mobile Platform 1 Pose: [0.790290224340521;-9.97314467564082;-6.65551128312675]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.92108236840886;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85526669733392;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97279440592416;6.37401705925529]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01922505665564;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97739414003947;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.676320e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.021476e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 595:
Mobile Platform 1 Pose: [0.790290224340521;-9.97180137845838;-6.66226151132521]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.13600991759973;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.07019424652478;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97379737335125;6.37544369816138]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.85875026832168;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.81691935170551;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.673173e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.100731e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 596:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362592696263;-6.65292838477481]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84408215691919;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77826648584424;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202229886515;6.37269922120761]
Robot Arm 2 Joint Angles: [0.00256857777095261;-2.14276218609733;0.00256857627675974;0.00650161998608432;0.00256857627679671;2.10093126948116;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.223978e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.136050e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 597:
Mobile Platform 1 Pose: [0.790290224340521;-9.97175807581849;-6.66261161588518]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.14293833998295;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.077122668908;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395673699406;6.37581045531416]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83325208547167;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.7914211688555;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.923979e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.571314e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 598:
Mobile Platform 1 Pose: [0.790290224340521;-9.97320613504879;-6.65503534258832]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.91124886313435;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.84543319205941;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97245723104834;6.37324435527195]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07317303678711;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03134212017094;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.933088e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.027877e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 599:
Mobile Platform 1 Pose: [0.790290224340521;-9.97269005790244;-6.65791728685056]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.9938212065508;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.92800553547585;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97317250131372;6.37457580130494]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95872979432514;0.00256857627675973;0.00650161998608437;0.00256857627679669;1.91689887770897;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.601655e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.073344e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 600:
Mobile Platform 1 Pose: [0.790290224340521;-9.97218494428875;-6.66031169367649]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07463938474122;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00882371366627;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.9735307732786;6.37492835273801]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90140627994442;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.85957536332825;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.051295e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.282961e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 601:
Mobile Platform 1 Pose: [0.790290224340521;-9.97351501623167;-6.65360564010285]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86182787387334;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79601220279839;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9721698328985;6.37291770727069]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11915674076072;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.07732582414455;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.218753e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.277470e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 602:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162272126859;-6.66329672147135]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16459506796619;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09877939689124;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401625735131;6.37581961144643]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.82372882831175;0.00256857627675973;0.00650161998608442;0.00256857627679669;1.78189791169558;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.859804e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.252255e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 603:
Mobile Platform 1 Pose: [0.790290224340521;-9.97350623421079;-6.65355768379866]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.86323299721533;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.79741732614038;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97217872281787;6.37279214744589]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11773435366141;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.07590343704524;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.231598e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.981901e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 604:
Mobile Platform 1 Pose: [0.790290224340521;-9.97220413636469;-6.66044336505423]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.07156865259049;-0.00246155353175018;-0.00729489384718761;-0.00246155353175018;-2.00575298151554;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97351815361566;6.37507571417961]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90342542601562;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86159450939945;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.028169e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.404044e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 605:
Mobile Platform 1 Pose: [0.790290224340521;-9.9726694762697;-6.65785667226303]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.99711426778992;-0.00246155353175017;-0.00729489384718764;-0.00246155353175017;-1.93129859671498;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97318745372924;6.37431013590445]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95633740784214;0.00256857627675973;0.00650161998608437;0.00256857627679669;1.91450649122597;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.934359e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.053045e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 606:
Mobile Platform 1 Pose: [0.790290224340521;-9.97322199209643;-6.65521599295489]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.90871173551312;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.84289606443818;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97244407998263;6.37332754088115]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07527720730029;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03344629068412;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.148612e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.035394e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 607:
Mobile Platform 1 Pose: [0.790290224340521;-9.9717474433974;-6.66265671457679]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.14463952735645;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.0788238562815;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97396136217907;6.37563572343616]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83251205586906;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79068113925289;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.996114e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.143608e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 608:
Mobile Platform 1 Pose: [0.790290224340521;-9.97362514107481;-6.65303293800107]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84420789897167;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77839222789672;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202542221777;6.37254434689204]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.14226244967711;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10043153306094;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.982468e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.722466e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 609:
Mobile Platform 1 Pose: [0.790290224340521;-9.9718138934825;-6.66244718488557]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.13400751374104;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.0681918426661;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97378844636943;6.37544637640872]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.86017858541197;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.8183476687958;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.393525e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.745198e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 610:
Mobile Platform 1 Pose: [0.790290224340521;-9.97312769825174;-6.6555479841257]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.92379875066221;-0.00246155353175017;-0.00729489384718767;-0.00246155353175017;-1.85798307958727;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97280964662549;6.37366184470359]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01678654444255;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97495562782638;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.980731e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.144881e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 611:
Mobile Platform 1 Pose: [0.790290224340521;-9.97278677245363;-6.65752990957887]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.97834687836006;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.91253120728512;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97279904524172;6.37385674809565]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01848276584626;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97665184923009;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.853197e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.301830e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 612:
Mobile Platform 1 Pose: [0.790290224340521;-9.97209659981066;-6.66088048183774]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.08877450123501;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.02295883016006;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97379468939062;6.37527102263566]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.85917970202151;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.81734878540534;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.749115e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.571184e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 613:
Mobile Platform 1 Pose: [0.790290224340521;-9.97355220924746;-6.65351287251039]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85587699134773;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.79006132027279;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202322973835;6.37253335419952]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.14261324638459;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10078232976842;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.724163e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.236852e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 614:
Mobile Platform 1 Pose: [0.790290224340521;-9.97162911988588;-6.66338566308771]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16357128920088;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09775561812594;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395816979345;6.37564455848835]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83302283756903;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79119192095286;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.796824e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.176860e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 615:
Mobile Platform 1 Pose: [0.790290224340521;-9.97346026387818;-6.65389973694696]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87058825043294;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.804772579358;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97245323303333;6.37307060043675]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07381271918858;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03198180257241;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.537093e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.679419e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 616:
Mobile Platform 1 Pose: [0.790290224340521;-9.97229731248759;-6.66008205268815]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.05666047292679;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.99084480185185;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97317706110591;6.37441527859164]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95800022757518;0.00256857627675973;0.00650161998608437;0.00256857627679669;1.91616931095901;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.020594e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.334988e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 617:
Mobile Platform 1 Pose: [0.790290224340521;-9.97257128677573;-6.65847368287552]
Robot Arm 1 Joint Angles: [-0.00246155353174373;2.01282458682362;-0.00246155353175017;-0.00729489384718763;-0.00246155353175017;-1.94700891574867;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97352696644245;6.37475406766887]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90201537372978;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86018445711361;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.427364e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.866256e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 618:
Mobile Platform 1 Pose: [0.790290224340521;-9.97329353011835;-6.6549492827296]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.89726565200458;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.83144998092964;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97217251937036;6.3727545084938]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11872690526381;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.07689598864764;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.022942e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.402542e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 619:
Mobile Platform 1 Pose: [0.790290224340521;-9.97170258959016;-6.66300595824169]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15181613651554;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.0860004654406;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97401628965918;6.37565132354372]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.8237236590526;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.78189274243643;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.832677e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.034484e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 620:
Mobile Platform 1 Pose: [0.790290224340521;-9.97361672251703;-6.65317750822343]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.84555486821656;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.77973919714162;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97217601760926;6.37262057485453]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.11816718703905;0.00256857627675974;0.00650161998608432;0.0025685762767967;2.07633627042288;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.246276e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.733942e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 621:
Mobile Platform 1 Pose: [0.790290224340521;-9.9718780449618;-6.66223956782188]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.12374327705323;-0.00246155353175018;-0.00729489384718759;-0.00246155353175018;-2.05792760597829;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97352200840204;6.37491397197774]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.90280866019552;0.00256857627675973;0.00650161998608439;0.00256857627679669;1.86097774357935;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.745899e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.547331e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 622:
Mobile Platform 1 Pose: [0.790290224340521;-9.97304409970937;-6.65608772173601]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.93717451744195;-0.00246155353175017;-0.00729489384718766;-0.00246155353175017;-1.87135884636701;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.973182936095;6.37413500479242]
Robot Arm 2 Joint Angles: [0.0025685777709526;-1.95706022932081;0.00256857627675973;0.00650161998608437;0.0025685762767967;1.91522931270464;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.314846e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.833740e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 623:
Mobile Platform 1 Pose: [0.790290224340521;-9.97288122698983;-6.65715247219956]
Robot Arm 1 Joint Angles: [-0.00246155353174373;1.96323415256772;-0.00246155353175017;-0.00729489384718765;-0.00246155353175017;-1.89741848149278;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97244806761244;6.37316629014409]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.07463918653102;0.00256857627675974;0.00650161998608434;0.0025685762767967;2.03280826991485;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.205289e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.386880e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 624:
Mobile Platform 1 Pose: [0.790290224340521;-9.97201330469422;-6.66142389990012]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.10210171986555;-0.00246155353175018;-0.0072948938471876;-0.00246155353175018;-2.0362860487906;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395999604467;6.37546511580368]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.83273063737364;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79089972075747;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.899450e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.723725e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 625:
Mobile Platform 1 Pose: [0.790290224340521;-9.97358198172603;-6.65345834245243]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.85111339477594;-0.00246155353175017;-0.0072948938471877;-0.00246155353175017;-1.78529772370099;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202446485603;6.37237555783516]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.14241562755583;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.10058471093966;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.377132e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.166464e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 626:
Mobile Platform 1 Pose: [0.790290224340521;-9.97164598478172;-6.66342121177268]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.16087290586561;-0.00246155353175018;-0.00729489384718757;-0.00246155353175018;-2.09505723479067;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97379118211283;6.37528271916369]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.8597408664691;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.81790994985293;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.999646e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.962977e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 627:
Mobile Platform 1 Pose: [0.790290224340521;-9.97340720052527;-6.65427909486872]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.87907838689761;-0.00246155353175017;-0.00729489384718769;-0.00246155353175017;-1.81326271582266;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.9728050397728;6.37348687947949]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01752364087297;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97569272425681;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.076996e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.233661e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 628:
Mobile Platform 1 Pose: [0.790290224340521;-9.97239331136782;-6.65970484774457]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.04130065208955;-0.00246155353175018;-0.00729489384718762;-0.00246155353175018;-1.97548498101461;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97280367114284;6.37369641358799]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.01774262166679;0.00256857627675974;0.00650161998608435;0.0025685762767967;1.97591170505062;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.451387e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.055210e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 629:
Mobile Platform 1 Pose: [0.790290224340521;-9.97247293193896;-6.65909196584148]
Robot Arm 1 Joint Angles: [-0.00246155353174374;2.02856136070697;-0.00246155353175018;-0.00729489384718763;-0.00246155353175018;-1.96274568963203;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97379199988745;6.37509834125535]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.85961002252931;0.00256857627675973;0.0065016199860844;0.00256857627679669;1.81777910591314;0.0025685769679763]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.527418e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.361101e-267。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 630:
Mobile Platform 1 Pose: [0.790290224340521;-9.97335884375426;-6.65471363263158]
Robot Arm 1 Joint Angles: [-0.00246155353174372;1.88681547025955;-0.00246155353175017;-0.00729489384718768;-0.00246155353175017;-1.8209997991846;-0.00246155343126115]
Mobile Platform 2 Pose: [-2.36113416893958;9.97202418035031;6.37236751503906]
Robot Arm 2 Joint Angles: [0.0025685777709526;-2.14246114847127;0.00256857627675974;0.00650161998608431;0.00256857627679671;2.1006302318551;0.00256857696797631]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.713140e-267。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.414198e-267。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 631:
Mobile Platform 1 Pose: [0.790290224340521;-9.97166734258151;-6.66330653418026]
Robot Arm 1 Joint Angles: [-0.00246155353174375;2.15745565789911;-0.00246155353175018;-0.00729489384718758;-0.00246155353175018;-2.09163998682416;-0.00246155343126116]
Mobile Platform 2 Pose: [-2.36113416893958;9.97395958055641;6.3754786253065]
Robot Arm 2 Joint Angles: [0.00256857777095259;-1.8327971154955;0.00256857627675973;0.00650161998608441;0.00256857627679669;1.79096619887933;0.0025685769679763]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/plot/real_time_2D_plot.m',49,1)">49  </a>end
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbstep; end
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 Joint Angles: [-0.000121114227682437;0.185441233024398;-0.000121114227682768;1.86264489701995e-09;-0.000121114227682768;-0.225185848658087;-0.000121114227682768]
Mobile Platform 2 Pose: [-2.35619437191491;9.99866771753545;9.81844390937036]
Robot Arm 2 Joint Angles: [0.000121114227682424;-0.185441233024398;0.000121114227682774;-1.86264487453752e-09;0.000121114227682774;0.213165194053599;0.000121114227682774]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814488;-9.99718189953243;-9.65601765681]
Robot Arm 1 Joint Angles: [-0.000242257227146163;0.370882466048795;-0.000242257227146826;-0.00011813576366936;-0.000242257227146826;-0.450777934956315;-0.000242257227146826]
Mobile Platform 2 Pose: [-2.35619425360615;9.99733167719312;9.63690106041641]
Robot Arm 2 Joint Angles: [0.000242260535616737;-0.370882466048795;0.000242260535617437;8.24406669025269e-05;0.000242260535617437;0.426849204164602;0.000242260535617437]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 Joint Angles: [-0.000363286279800703;0.556323699073193;-0.000363286279801698;-0.000357282809229025;-0.000363286279801698;-0.676844213293331;-0.000363286279801698]
Mobile Platform 2 Pose: [-2.35640296489661;9.99599115571379;9.455454545559]
Robot Arm 2 Joint Angles: [0.000363412210349681;-0.556323699073193;0.000363412210350751;0.000250017184069794;0.000363412210350751;0.641165057095133;0.000363412210350751]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 Joint Angles: [-0.000483866411479533;0.74176493209759;-0.000483866411480859;-0.00071951007623215;-0.000483866411480859;-0.903917115513837;-0.000483866411480859]
Mobile Platform 2 Pose: [-2.35659595306927;9.99464273325087;9.27450999073585]
Robot Arm 2 Joint Angles: [0.000484380097856905;-0.74176493209759;0.000484380097858423;0.000505156626641499;0.000484380097858423;0.856657475359012;0.000484380097858423]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 Joint Angles: [-0.000603674592608101;0.927206165121988;-0.000603674592609757;-0.00120731922081315;-0.000603674592609757;-1.13251733087994;-0.000603674592609757]
Mobile Platform 2 Pose: [-2.35677811216098;9.99328288963676;9.09451139733659]
Robot Arm 2 Joint Angles: [0.000604983142434488;-0.927206165121988;0.000604983142436368;0.000851201656280582;0.000604983142436368;1.07388626252499;0.000604983142436368]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346624]
Robot Arm 1 Joint Angles: [-0.000722796083224461;1.11264739814639;-0.000722796083226446;-0.00182553790636017;-0.000722796083226446;-1.36238137284088;-0.000722796083226446]
Mobile Platform 2 Pose: [-2.3569574015238;9.99191236748666;8.91540822412511]
Robot Arm 2 Joint Angles: [0.000725412492363571;-1.11264739814639;0.000725412492365748;0.00129364175131269;0.000725412492365748;1.29272755445515;0.000725412492365748]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528708;-9.99002831581144;-8.79949883355178]
Robot Arm 1 Joint Angles: [-0.00084158055635903;1.29808863117078;-0.000841580556361343;-0.00257974514714835;-0.000841580556361343;-1.59288971083287;-0.000841580556361343]
Mobile Platform 2 Pose: [-2.35714150068548;9.99053429959966;8.73681877399268]
Robot Arm 2 Joint Angles: [0.000846112372791619;-1.29808863117078;0.000846112372794161;0.00183859832561985;0.000846112372794161;1.51267351308019;0.000846112372794161]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274174]
Robot Arm 1 Joint Angles: [-0.00095982998704643;1.48352986419518;-0.00095982998704907;-0.00347025803777757;-0.00095982998704907;-1.82450740028498;-0.00095982998704907]
Mobile Platform 2 Pose: [-2.35733477380413;9.98914657460175;8.55900221813084]
Robot Arm 2 Joint Angles: [0.00096709893406065;-1.48352986419518;0.000967098934063465;0.00248882907447883;0.000967098934063465;1.73405944325886;0.000967098934063465]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196128;-9.98711511781;-8.46062525935788]
Robot Arm 1 Joint Angles: [-0.00107743739450421;1.66897109721958;-0.00107743739450718;-0.00449854422257735;-0.00107743739450718;-2.05708258800973;-0.00107743739450718]
Mobile Platform 2 Pose: [-2.35733465527977;9.98774927785345;8.38210069631835]
Robot Arm 2 Joint Angles: [0.0010884660169618;-1.66897109721958;0.00108846601696495;0.00324798288864984;0.00108846601696495;1.95686776711348;0.00108846601696495]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929674;-9.98565931998689;-8.29072857539469]
Robot Arm 1 Joint Angles: [-0.00119546838173931;1.85441233024398;-0.00119546838174261;-0.00567665166884604;-0.00119546838174261;-2.0943951023932;-0.00119546838174261]
Mobile Platform 2 Pose: [-2.35733453546672;9.9863505361838;8.20502717842039]
Robot Arm 2 Joint Angles: [0.00121115271572056;-1.85441233024398;0.00121115271571719;0.00412540870764538;0.00121115271572405;2.0943951023932;0.00121115271572405]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.98418232823806;-8.12462828169963]
Robot Arm 1 Joint Angles: [-0.00131696062399761;2.03985356326837;-0.00131696062400122;-0.00639922022289152;-0.00131696062400122;-2.30399143685392;-0.00131696052349854]
Mobile Platform 2 Pose: [-2.35733441519971;9.98493706036823;8.03015833868041]
Robot Arm 2 Joint Angles: [0.00133430534029446;-2.03985356326837;0.00133430534029053;0.00485297554163648;0.0013343053402983;2.29681199039171;0.00133430420994034]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840343631;-7.95994828885485]
Robot Arm 1 Joint Angles: [-0.00143870881546709;2.14949927567335;-0.00143870881547102;-0.00719336612686439;-0.00143870881547102;-2.22826414942432;-0.00143870871496834]
Mobile Platform 2 Pose: [-2.35744418408676;9.98351845686627;7.85621816925451]
Robot Arm 2 Joint Angles: [0.00145850066498858;-2.14635912544803;0.00145850066498017;0.00563270252640432;0.00145850066499275;2.22297881661186;0.00145849953463479]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455233642;-7.79314279534789]
Robot Arm 1 Joint Angles: [-0.00156630058254284;2.18077941225183;-0.00156630058254709;-0.00742071387543599;-0.00156630058254709;-2.20427246494567;-0.00156630048204441]
Mobile Platform 2 Pose: [-2.35818458529929;9.98211905509671;7.68190745897923]
Robot Arm 2 Joint Angles: [0.00158531814136949;-2.17722325628743;0.0015853181413566;0.00585691934163223;0.00158531814137399;2.20007463276513;0.00158531701101604]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.97972356094619;-7.63794018920194]
Robot Arm 1 Joint Angles: [-0.00168692887755489;2.19333238381216;-0.00168692887755944;-0.00746871707029978;-0.00168692887755944;-2.20035149692109;-0.00168692877705676]
Mobile Platform 2 Pose: [-2.35938581371466;9.98066897536772;7.51573462487952]
Robot Arm 2 Joint Angles: [0.00170725705555177;-2.1888628394338;0.0017072570555344;0.00590848439440483;0.0017072570555566;2.19568731256314;0.00170725592519864]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591664647;-7.46636918682404]
Robot Arm 1 Joint Angles: [-0.00182021590843541;2.1916426546686;-0.00182021590844029;-0.0074668554106574;-0.00182021590844029;-2.19374950637937;-0.00182021580793761]
Mobile Platform 2 Pose: [-2.36086926158793;9.97930469290192;7.33819197999495]
Robot Arm 2 Joint Angles: [0.00183886993576416;-2.18822185028754;0.00183886993574232;0.00591137331618888;0.00183886993576933;2.19026745809096;0.00183886880541137]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346213;-9.97678818483778;-7.31817266006872]
Robot Arm 1 Joint Angles: [-0.00193743348788856;2.19689567720104;-0.00193743348789372;-0.00745079646704906;-0.00193743348789372;-2.19753736886275;-0.00193743338739104]
Mobile Platform 2 Pose: [-2.36112615798283;9.97782853878928;7.17876232197245]
Robot Arm 2 Joint Angles: [0.00195773378000167;-2.33644247948293;0.00195773377997889;0.00589977337269694;0.00195773378000759;2.34901943517239;0.00195773264964964]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232726;-9.9753590119527;-7.16418511940773]
Robot Arm 1 Joint Angles: [-0.00205940439551018;2.19373921816966;-0.00205940439551565;-0.00742961274493574;-0.00205940439551565;-2.19394443668375;-0.00205940429501297]
Mobile Platform 2 Pose: [-2.36283278647673;9.97646850404151;7.0081794308335]
Robot Arm 2 Joint Angles: [0.00208719651535111;-2.27192169726706;0.00208719651532385;0.00592112181355196;0.00208719651535736;2.23605767328133;0.0020871953849994]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785396026760021;-9.9743099077894;-7.00084677753696]
Robot Arm 1 Joint Angles: [-0.00218788714244672;2.17454821889181;-0.0021878871424525;-0.00740588626491103;-0.0021878871424525;-2.17462402111636;-0.00218788704194981]
Mobile Platform 2 Pose: [-2.36455654089934;9.97524771519591;6.84547917517208]
Robot Arm 2 Joint Angles: [0.00221134722492008;-2.20621208236742;0.00221134722488833;0.00579157456003554;0.00221134722492664;2.19553060430613;0.00221134609456869]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785395956603331;-9.97259808477927;-6.91581468782267]
Robot Arm 1 Joint Angles: [-0.00225972628881696;2.20060448107323;-0.00225972628882291;-0.00739236729819143;-0.00225972628882291;-2.20063532500117;-0.00225972618832022]
Mobile Platform 2 Pose: [-2.36624804567758;9.97367782452114;6.73209007359761]
Robot Arm 2 Joint Angles: [0.0022974182516261;-2.20328486895757;0.00229741825159457;0.00575591246600168;0.00229741825163288;2.20010181355313;0.00229741712127492]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227854;-9.97409553094492;-6.77330520536714]
Robot Arm 1 Joint Angles: [-0.00236353291585721;2.01516324804883;-0.00236353291586342;-0.00737275757568851;-0.00236353291586342;-1.96106355016073;-0.00236353281536074]
Mobile Platform 2 Pose: [-2.36624791748038;9.97452288853706;6.56520765455505]
Robot Arm 2 Joint Angles: [0.00242869218234563;-2.06804500790655;0.00242869218231443;0.00572629396333566;0.00242869218235274;2.06492118950744;0.00242869105199478]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.9722812970083;-6.73833367380644]
Robot Arm 1 Joint Angles: [-0.00239739691375578;2.0943950974136;-0.00239739691376206;-0.00731934997979104;-0.00239739691376206;-2.09439511080696;-0.00239739681325938]
Mobile Platform 2 Pose: [-2.36765347886643;9.97263865161925;6.50934404981404]
Robot Arm 2 Joint Angles: [0.00246982443095042;-2.17693058015814;0.00246982443091933;0.0057169847567504;0.00246982443095764;2.17599900548068;0.00246982330059968]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540073;-9.97333331320683;-6.6973716673834]
Robot Arm 1 Joint Angles: [-0.00242465134050779;1.92606731995396;-0.00242465134051415;-0.00731416428434603;-0.00242465134051415;-1.92607770473821;-0.00242465124001147]
Mobile Platform 2 Pose: [-2.36780153414366;9.97413547811174;6.46423095238477]
Robot Arm 2 Joint Angles: [0.00250733389907102;-1.99148934713375;0.00250733389903999;0.00571065014074989;0.0025073338990783;1.93651307406636;0.00250733276872035]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.78539578119585;-9.97294909647023;-6.68023583492612]
Robot Arm 1 Joint Angles: [-0.00243933982504677;1.98753916232563;-0.00243933982505317;-0.00731132879957033;-0.00243933982505317;-1.98755521807943;-0.00243933972455049]
Mobile Platform 2 Pose: [-2.36857745213687;9.9731828276043;6.43932529384444]
Robot Arm 2 Joint Angles: [0.00252535883326286;-2.0943951023932;0.00252535883323187;0.00568260674019762;0.00252535883327018;2.08896465489929;0.00252535770291223]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.786338791775456;-9.97200303867874;-6.67348000701647]
Robot Arm 1 Joint Angles: [-0.00244810023692547;2.0943951023932;-0.00244810023693188;-0.00730965013647538;-0.00244810023693188;-2.0943951023932;-0.00244810013643561]
Mobile Platform 2 Pose: [-2.36847485613367;9.97296715367594;6.42499278893334]
Robot Arm 2 Joint Angles: [0.00253648525456974;-2.0943951023932;0.00253648525453879;0.00567945249320565;0.0025364852545771;2.0943951023932;0.00253648412421914]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.787575037759478;-9.9735848863904;-6.65848978106497]
Robot Arm 1 Joint Angles: [-0.00245346307467694;1.90895386920815;-0.00245346307468338;-0.0073086314547742;-0.00245346307468338;-1.84130049095738;-0.0024534629741871]
Mobile Platform 2 Pose: [-2.36847484971132;9.97393646537772;6.41802220528938]
Robot Arm 2 Joint Angles: [0.00254306174385624;-1.93930427217471;0.0025430617438253;0.00567849456002194;0.00254306174386361;1.93930618804108;0.00254306061350565]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.788810177527241;-9.97165007570355;-6.66429508260724]
Robot Arm 1 Joint Angles: [-0.00245662747227496;2.09439509895251;-0.00245662747228139;-0.00730247937426817;-0.00245662747228139;-2.09439510091386;-0.00245662737179199]
Mobile Platform 2 Pose: [-2.36847484602095;9.9721582248379;6.41038517769028]
Robot Arm 2 Joint Angles: [0.00254683881383718;-2.09439509589475;0.00254683881380625;0.00567793839338638;0.00254683881384456;2.09439510336863;0.00254683768348661]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.790044329088375;-9.97334587556591;-6.65299731027004]
Robot Arm 1 Joint Angles: [-0.00245854358539268;1.90895386626445;-0.00245854358539913;-0.00730211693673944;-0.00245854358539913;-1.82306748925736;-0.00245854348490973]
Mobile Platform 2 Pose: [-2.36847484378466;9.97408627609977;6.41058489022753]
Robot Arm 2 Joint Angles: [0.00254912691434267;-1.90895386239922;0.00254912691431175;0.00567760671082536;0.00254912691435006;1.79987079202919;0.0025491257839921]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.790044327941186;-9.97254300154296;-6.65580134984087]
Robot Arm 1 Joint Angles: [-0.00245971830652837;2.03741087313589;-0.00245971830653481;-0.00729928013591849;-0.00245971830653481;-1.95153016973044;-0.00245971820604541]
Mobile Platform 2 Pose: [-2.36847484241068;9.97268217684236;6.40633802936288]
Robot Arm 2 Joint Angles: [0.00255053386446947;-2.13360557419041;0.00255053386443856;0.00567343731583779;0.00255053386447687;2.02453084261036;0.00255053273411891]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.790044327238374;-9.97237811514215;-6.655505845517]
Robot Arm 1 Joint Angles: [-0.00246043798669423;2.06379097894252;-0.00246043798670067;-0.00729756181293154;-0.00246043798670067;-1.97791371218305;-0.00246043788621127]
Mobile Platform 2 Pose: [-2.36847484157705;9.97335928332643;6.40652727823509]
Robot Arm 2 Joint Angles: [0.00255138750544962;-2.02526605668715;0.0025513875054187;0.00567095726502492;0.00255138750545702;1.91619628520873;0.00255138637509906]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.790044326816292;-9.97341831893772;-6.64975922898104]
Robot Arm 1 Joint Angles: [-0.00246087019856569;1.89735734202993;-0.00246087019857215;-0.00729653219098246;-0.00246087019857215;-1.81148213451436;-0.00246087009808275]
Mobile Platform 2 Pose: [-2.36847484107324;9.97361700511181;6.40605776338971]
Robot Arm 2 Joint Angles: [0.00255190340836714;-1.98402905839648;0.00255190340833622;0.00566944463405737;0.00255190340837453;1.87496231217999;0.00255190227801658]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.790044326552647;-9.97165700520948;-6.65837188254433]
Robot Arm 1 Joint Angles: [-0.00246114017046923;2.17916688599037;-0.00246114017047567;-0.00729587963371386;-0.00246114017047567;-2.09329298358933;-0.00246114006998627]
Mobile Platform 2 Pose: [-2.36847484076225;9.97243409382879;6.40393689731797]
Robot Arm 2 Joint Angles: [0.00255222186013303;-2.17329392657227;0.00255222186010212;0.00566850752700403;0.00255222186014043;2.06422905456988;0.00255222072978247]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.790044326396365;-9.97359339874161;-6.64819538521659]
Robot Arm 1 Joint Angles: [-0.00246130020392308;1.86934354316568;-0.00246130020392954;-0.00729550194958319;-0.00246130020392954;-1.78347039613291;-0.00246130010344014]
Mobile Platform 2 Pose: [-2.3684748405778;9.97414171932385;6.40634642536804]
Robot Arm 2 Joint Angles: [0.0025524107389631;-1.90007330043307;0.00255241073893218;0.0056679605972578;0.00255241073897049;1.79100952229018;0.00255240960861254]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.79004432629869;-9.97198118846758;-6.65650196395322]
Robot Arm 1 Joint Angles: [-0.0024614002226815;2.12729694463558;-0.00246140022268794;-0.00729525957563806;-0.00246140022268794;-2.0414242823507;-0.00246140012219853]
Mobile Platform 2 Pose: [-2.36847484046261;9.97220849208409;6.40307573296746]
Robot Arm 2 Joint Angles: [0.00255252869366636;-2.20938930901158;0.00255252869363544;0.00566761081500247;0.00255252869367375;2.1003262304332;0.0025525275633158]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.790044326240075;-9.97295374698651;-6.65125353928843]
Robot Arm 1 Joint Angles: [-0.00246146024479644;1.97168743937331;-0.0024614602448029;-0.00729511734176397;-0.0024614602448029;-1.88581506155618;-0.00246146014431349]
Mobile Platform 2 Pose: [-2.36847484039435;9.97393200051278;6.40583526615424]
Robot Arm 2 Joint Angles: [0.00255259858871285;-1.93362775860635;0.00255259858868193;0.00566740899950139;0.00255259858872024;1.82456508365897;0.00255259745836229]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.790044326204335;-9.97296922306273;-6.65134945214438]
Robot Arm 1 Joint Angles: [-0.00246149684208728;1.96921117930725;-0.00246149684209373;-0.00729502947106574;-0.00246149684209373;-1.88333897723151;-0.00246149674160433]
Mobile Platform 2 Pose: [-2.36847484035223;9.97291482562975;6.40393815186328]
Robot Arm 2 Joint Angles: [0.00255264172657165;-2.09637561236933;0.00255264172654074;0.0056672814782701;0.00255264172657905;1.98731319246441;0.00255264059622109]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.79004432618224;-9.97196650279706;-6.65629097987546]
Robot Arm 1 Joint Angles: [-0.00246151946805974;2.12964636747806;-0.00246151946806618;-0.00729497513398379;-0.00246151946806618;-2.04377427407649;-0.00246151936757678]
Mobile Platform 2 Pose: [-2.36847484032672;9.97308870337529;6.40439069228773]
Robot Arm 2 Joint Angles: [0.00255266784155471;-2.06855509698758;0.0025526678415238;0.00566720538156407;0.00255266784156211;1.95949282927608;0.00255266671120415]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.790044326169113;-9.97359580839791;-6.64798630851878]
Robot Arm 1 Joint Angles: [-0.00246153291000035;1.8689574394943;-0.00246153291000681;-0.00729494328697541;-0.00246153291000681;-1.78308540978674;-0.00246153280951741]
Mobile Platform 2 Pose: [-2.3684748403114;9.97381866708714;6.40534297564955]
Robot Arm 2 Joint Angles: [0.00255268352988366;-1.95176085726816;0.00255268352985275;0.00566715955908728;0.00255268352989106;1.84269868120161;0.0025526823995331]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.790044326160865;-9.9716595872394;-6.6579482771418]
Robot Arm 1 Joint Angles: [-0.00246154135612548;2.17875280438829;-0.00246154135613192;-0.007294922819115;-0.00246154135613192;-2.09288081561645;-0.00246154125564252]
Mobile Platform 2 Pose: [-2.36847484030188;9.97227567518361;6.40300245350216]
Robot Arm 2 Joint Angles: [0.00255269327728187;-2.19863953306616;0.00255269327725096;0.00566713079256123;0.00255269327728927;2.08957741453266;0.00255269214693132]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.790044326155973;-9.97340627533404;-6.64887147269363]
Robot Arm 1 Joint Angles: [-0.00246154636459813;1.89928269742594;-0.00246154636460458;-0.0072949109988625;-0.00246154636460458;-1.81341073229461;-0.00246154626411518]
Mobile Platform 2 Pose: [-2.36847484029626;9.97415865617035;6.40597122173233]
Robot Arm 2 Joint Angles: [0.00255269903279277;-1.89736255855633;0.00255269903276185;0.00566711416031249;0.00255269903280017;1.78830047328733;0.00255269790244221]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.79004432615294;-9.97238834246987;-6.65431361128152]
Robot Arm 1 Joint Angles: [-0.00246154947082558;2.06215194818688;-0.00246154947083202;-0.00729490349227655;-0.00246154947083202;-1.97627999806872;-0.00246154937034262]
Mobile Platform 2 Pose: [-2.36847484029275;9.97232129240589;6.40293846961236]
Robot Arm 2 Joint Angles: [0.0025527026287542;-2.19134075020382;0.00255270262872329;0.00566710349431316;0.0025527026287616;2.08227868626681;0.00255270149840364]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.790044326151098;-9.9725122046687;-6.6534709037986]
Robot Arm 1 Joint Angles: [-0.00246155135736861;2.04233399187867;-0.00246155135737506;-0.00729489899778771;-0.00246155135737506;-1.95646205074949;-0.00246155125688566]
Mobile Platform 2 Pose: [-2.36847484029066;9.97375447260305;6.40535995801188]
Robot Arm 2 Joint Angles: [0.00255270476597127;-1.96203191247899;0.00255270476594036;0.00566709731466221;0.00255270476597867;1.85296986090129;0.00255270363562072]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.790044326149989;-9.97332888089895;-6.64945245482069]
Robot Arm 1 Joint Angles: [-0.00246155249264476;1.91166579232939;-0.00246155249265121;-0.00729489628858751;-0.00246155249265121;-1.82579385661861;-0.00246155239216181]
Mobile Platform 2 Pose: [-2.36847484028938;9.97317646212617;6.4042024700106]
Robot Arm 2 Joint Angles: [0.00255270607674743;-2.05451358491547;0.00255270607671651;0.00566709344992581;0.00255270607675482;1.94545154106725;0.00255270494639687]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.790044326149296;-9.97169930640173;-6.65771343787771]
Robot Arm 1 Joint Angles: [-0.00246155320190272;2.17239771017356;-0.00246155320190916;-0.00729489457650261;-0.00246155320190916;-2.08652577788694;-0.00246155310141975]
Mobile Platform 2 Pose: [-2.3684748402886;9.97282867052621;6.40385756640478]
Robot Arm 2 Joint Angles: [0.00255270687632643;-2.11016023857017;0.00255270687629552;0.00566709111169571;0.00255270687633383;2.0010981993984;0.00255270574597587]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.790044326148887;-9.97361778140385;-6.64785930275082]
Robot Arm 1 Joint Angles: [-0.00246155362140027;1.86544170884388;-0.00246155362140672;-0.00729489358597807;-0.00246155362140672;-1.77956977853832;-0.00246155352091732]
Mobile Platform 2 Pose: [-2.36847484028814;9.97397964158823;6.40553816848775]
Robot Arm 2 Joint Angles: [0.00255270735352011;-1.92600486725821;0.00255270735348919;0.00566708972280697;0.0025527073535275;1.81694283086422;0.00255270622316955]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.79004432614863;-9.97187540284973;-6.65693911168932]
Robot Arm 1 Joint Angles: [-0.00246155388442602;2.1442222768647;-0.00246155388443246;-0.00729489294837049;-0.00246155388443246;-2.05835034783435;-0.00246155378394306]
Mobile Platform 2 Pose: [-2.36847484028785;9.97218529268226;6.40275410117909]
Robot Arm 2 Joint Angles: [0.00255270765122541;-2.21310069133214;0.0025527076511945;0.00566708884213536;0.00255270765123281;2.1040386566995;0.00255270652087486]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.790044326148476;-9.97307917298712;-6.65059573373089]
Robot Arm 1 Joint Angles: [-0.00246155404161184;1.95161905451065;-0.00246155404161829;-0.00729489257639617;-0.00246155404161829;-1.86574712622425;-0.00246155394112888]
Mobile Platform 2 Pose: [-2.36847484028767;9.97412285374959;6.40586427601082]
Robot Arm 2 Joint Angles: [0.00255270782687797;-1.90309092005213;0.00255270782684705;0.00566708833510599;0.00255270782688536;1.79402888643354;0.00255270669652741]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.790044326148382;-9.97283566018047;-6.65207589134089]
Robot Arm 1 Joint Angles: [-0.00246155413788282;1.99058110334292;-0.00246155413788927;-0.00729489234483823;-0.00246155413788927;-1.90470917551964;-0.00246155403739987]
Mobile Platform 2 Pose: [-2.36847484028757;9.97249754508564;6.40311882497602]
Robot Arm 2 Joint Angles: [0.00255270793631339;-2.16314030596069;0.00255270793628248;0.00566708801068319;0.00255270793632079;2.05407827299095;0.00255270680596284]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036831e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.992822e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.790044326148324;-9.97208304299821;-6.65574905124996]
Robot Arm 1 Joint Angles: [-0.0024615541971627;2.11099985236249;-0.00246155419716914;-0.00729489220274249;-0.00246155419716914;-2.0251279248234;-0.00246155409667974]
Mobile Platform 2 Pose: [-2.3684748402875;9.97353659864007;6.40494795010897]
Robot Arm 2 Joint Angles: [0.00255270800171957;-1.99689173706264;0.00255270800168865;0.00566708782114043;0.00255270800172696;1.88782970447199;0.00255270687136901]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.810933e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.226797e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.79004432614829;-9.9735558981428;-6.64829031510854]
Robot Arm 1 Joint Angles: [-0.00246155423246117;1.87534302914477;-0.00246155423246763;-0.00729489211898641;-0.00246155423246763;-1.78947110177319;-0.00246155413197822]
Mobile Platform 2 Pose: [-2.36847484028746;9.97343243097508;6.40455002735712]
Robot Arm 2 Joint Angles: [0.00255270804153538;-2.01355856334433;0.00255270804150446;0.00566708770411463;0.00255270804154277;1.90449653098772;0.00255270691118482]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563688e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.302819e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.790044326148268;-9.97163424161902;-6.65815417285313]
Robot Arm 1 Joint Angles: [-0.00246155425463231;2.18280807289497;-0.00246155425463875;-0.00729489206528968;-0.00246155425463875;-2.09693614563078;-0.00246155415414934]
Mobile Platform 2 Pose: [-2.36847484028744;9.97258949124319;6.40340264968727]
Robot Arm 2 Joint Angles: [0.00255270806600713;-2.14842892037483;0.00255270806597622;0.00566708763229427;0.00255270806601453;2.03936688816187;0.00255270693565658]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056010e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.623889e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.790044326148255;-9.97348102354875;-6.64859200063384]
Robot Arm 1 Joint Angles: [-0.00246155426775955;1.88732296410816;-0.002461554267766;-0.00729489203431854;-0.002461554267766;-1.80145103690592;-0.0024615541672766]
Mobile Platform 2 Pose: [-2.36847484028743;9.97409093631199;6.4056865522976]
Robot Arm 2 Joint Angles: [0.00255270808053486;-1.90819770932473;0.00255270808050395;0.00566708759014386;0.00255270808054226;1.79913567719607;0.0025527069501843]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231714e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.689381e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.790044326148247;-9.97225080439178;-6.65511465899777]
Robot Arm 1 Joint Angles: [-0.00246155427592837;2.08415802920305;-0.00246155427593481;-0.00729489201455599;-0.00246155427593481;-1.99828610204033;-0.00246155417544541]
Mobile Platform 2 Pose: [-2.36847484028742;9.97216892593137;6.40263647184488]
Robot Arm 2 Joint Angles: [0.00255270808961829;-2.21571937019586;0.00255270808958738;0.00566708756322869;0.0025527080896257;2.10665733812103;0.00255270695926774]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531956e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.796898e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.790044326148243;-9.97265434071457;-6.65284314841671]
Robot Arm 1 Joint Angles: [-0.00246155428086843;2.01959221754569;-0.00246155428087487;-0.0072948920028084;-0.00246155428087487;-1.93372029040647;-0.00246155418038547]
Mobile Platform 2 Pose: [-2.36847484028741;9.97403402491772;6.40567688708604]
Robot Arm 2 Joint Angles: [0.00255270809498361;-1.91730353236433;0.00255270809495269;0.00566708754774464;0.00255270809499101;1.80824150032047;0.00255270696463305]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692336e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.229512e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.79004432614824;-9.97322813635381;-6.65009166086181]
Robot Arm 1 Joint Angles: [-0.00246155428385318;1.92778491525876;-0.00246155428385963;-0.00729489199567182;-0.00246155428385963;-1.84191298813381;-0.00246155418337023]
Mobile Platform 2 Pose: [-2.36847484028741;9.97272164877174;6.40338743334784]
Robot Arm 2 Joint Angles: [0.0025527080983113;-2.12728371571191;0.00255270809828039;0.00566708753789268;0.0025527080983187;2.01822168368775;0.00255270696796075]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663790e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.228766e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.790044326148238;-9.97176377605592;-6.65747833174334]
Robot Arm 1 Joint Angles: [-0.00246155428571269;2.16208256291683;-0.00246155428571913;-0.0072948919911896;-0.00246155428571913;-2.07621063580084;-0.00246155418522973]
Mobile Platform 2 Pose: [-2.36847484028741;9.97328887700825;6.40448913580265]
Robot Arm 2 Joint Angles: [0.00255270810031377;-2.03652719786403;0.00255270810028286;0.0056670875320727;0.00255270810032117;1.9274651658515;0.00255270696996322]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984383e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.087200e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.790044326148237;-9.97362810017476;-6.64792825122498]
Robot Arm 1 Joint Angles: [-0.00246155428681334;1.8637907039005;-0.0024615542868198;-0.00729489198858869;-0.0024615542868198;-1.77791877678971;-0.00246155418633039]
Mobile Platform 2 Pose: [-2.36847484028741;9.97366486078211;6.40487468262092]
Robot Arm 2 Joint Angles: [0.00255270810152349;-1.97636979404277;0.00255270810149257;0.00566708752852958;0.00255270810153089;1.86730776203734;0.00255270697117293]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059053e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.678635e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.790044326148236;-9.97178688173096;-6.65748585762159]
Robot Arm 1 Joint Angles: [-0.0024615542875044;2.15838565490653;-0.00246155428751084;-0.00729489198691316;-0.00246155428751084;-2.07251372779909;-0.00246155418702144]
Mobile Platform 2 Pose: [-2.36847484028741;9.972390481938;6.40300883954716]
Robot Arm 2 Joint Angles: [0.002552708102272;-2.18027040909827;0.00255270810224109;0.00566708752632547;0.0025527081022794;2.07120837709724;0.00255270697192145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857070e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.644639e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.790044326148236;-9.97319546009412;-6.65010816889694]
Robot Arm 1 Joint Angles: [-0.00246155428791612;1.93301311680005;-0.00246155428792258;-0.00729489198593997;-0.00246155428792258;-1.84714118969457;-0.00246155418743317]
Mobile Platform 2 Pose: [-2.3684748402874;9.97414974748061;6.40575090173761]
Robot Arm 2 Joint Angles: [0.00255270810271468;-1.89878792228022;0.00255270810268376;0.00566708752504442;0.00255270810272208;1.78972589028176;0.00255270697236412]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905722e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.276602e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.790044326148236;-9.97269674237362;-6.65290089960227]
Robot Arm 1 Joint Angles: [-0.00246155428816937;2.01280795207937;-0.00246155428817582;-0.00729489198532981;-0.00246155428817582;-1.9269360249751;-0.00246155418768642]
Mobile Platform 2 Pose: [-2.3684748402874;9.97222790056031;6.4026359645213]
Robot Arm 2 Joint Angles: [0.00255270810299157;-2.20628342952628;0.00255270810296066;0.00566708752422318;0.00255270810299897;2.09722139752946;0.00255270697264102]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634539e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.322005e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.790044326148235;-9.97221078236751;-6.65519589061502]
Robot Arm 1 Joint Angles: [-0.00246155428832465;2.09056155305592;-0.0024615542883311;-0.0072948919849583;-0.0024615542883311;-2.00468962595239;-0.00246155418784169]
Mobile Platform 2 Pose: [-2.3684748402874;9.97389397637641;6.40540169150999]
Robot Arm 2 Joint Angles: [0.00255270810315557;-1.93971129895096;0.00255270810312466;0.00566708752374958;0.00255270810316297;1.83064926695508;0.00255270697280502]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.066859e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.103214e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.790044326148235;-9.97350061208764;-6.64870144514554]
Robot Arm 1 Joint Angles: [-0.00246155428841736;1.88418879783617;-0.00246155428842382;-0.00729489198473799;-0.00246155428842382;-1.79831687073309;-0.00246155418793441]
Mobile Platform 2 Pose: [-2.3684748402874;9.97297503238647;6.40371193443376]
Robot Arm 2 Joint Angles: [0.0025527081032567;-2.08674233733995;0.00255270810322579;0.00566708752345077;0.0025527081032641;1.97768030534467;0.00255270697290615]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.068971e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.997103e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.790044326148235;-9.97163094257875;-6.65826439819488]
Robot Arm 1 Joint Angles: [-0.00246155428847557;2.18333591925693;-0.002461554288482;-0.00729489198459709;-0.002461554288482;-2.09746399215413;-0.0024615541879926]
Mobile Platform 2 Pose: [-2.3684748402874;9.9730270719895;6.40400528170771]
Robot Arm 2 Joint Angles: [0.00255270810331802;-2.07841600085578;0.00255270810328711;0.00566708752327196;0.00255270810332542;1.96935396886087;0.00255270697296746]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.775923e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.409709e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.790044326148235;-9.97354092047322;-6.64840098165678]
Robot Arm 1 Joint Angles: [-0.00246155428850995;1.87773945614168;-0.00246155428851641;-0.00729489198451601;-0.00246155428851641;-1.79186752903904;-0.00246155418802701]
Mobile Platform 2 Pose: [-2.3684748402874;9.97386039423227;6.40515042577263]
Robot Arm 2 Joint Angles: [0.00255270810335479;-1.94508444201214;0.00255270810332387;0.00566708752316468;0.00255270810336219;1.83602241001744;0.00255270697300423]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247122e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.512134e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.790044326148235;-9.97212037997975;-6.65588261795435]
Robot Arm 1 Joint Angles: [-0.00246155428853145;2.10502593509835;-0.0024615542885379;-0.00729489198446391;-0.0024615542885379;-2.01915400799581;-0.00246155418804849]
Mobile Platform 2 Pose: [-2.3684748402874;9.97224824887748;6.40270036121712]
Robot Arm 2 Joint Angles: [0.00255270810337768;-2.20302769877794;0.00255270810334677;0.00566708752309705;0.00255270810338508;2.09396566678337;0.00255270697302712]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064268e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.118263e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.790044326148235;-9.97279446095246;-6.65223038562863]
Robot Arm 1 Joint Angles: [-0.00246155428854438;1.99717297946416;-0.00246155428855083;-0.00729489198443325;-0.00246155428855083;-1.91130105236169;-0.00246155418806143]
Mobile Platform 2 Pose: [-2.3684748402874;9.97415507538703;6.40572556175719]
Robot Arm 2 Joint Angles: [0.00255270810339117;-1.89793545724976;0.00255270810336025;0.00566708752305814;0.00255270810339856;1.78887342525527;0.00255270697304061]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.743015e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.163508e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.790044326148235;-9.97311524563799;-6.65079329205333]
Robot Arm 1 Joint Angles: [-0.00246155428855222;1.94584742977855;-0.00246155428855868;-0.00729489198441446;-0.00246155428855868;-1.85997550267611;-0.00246155418806927]
Mobile Platform 2 Pose: [-2.3684748402874;9.97235727936041;6.40274677398261]
Robot Arm 2 Joint Angles: [0.00255270810339961;-2.18558282150934;0.0025527081033687;0.00566708752303304;0.00255270810340701;2.0765207895149;0.00255270697304906]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155939e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.135548e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.790044326148235;-9.9718470537216;-6.65714948839604]
Robot Arm 1 Joint Angles: [-0.00246155428855712;2.14875813640136;-0.00246155428856356;-0.00729489198440265;-0.00246155428856356;-2.06288620929895;-0.00246155418807415]
Mobile Platform 2 Pose: [-2.3684748402874;9.9737069697284;6.40504626543117]
Robot Arm 2 Joint Angles: [0.00255270810340462;-1.96963236263063;0.0025527081033737;0.00566708752301859;0.00255270810341201;1.86057033063622;0.00255270697305406]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302655e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.681363e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.790044326148235;-9.97362257675398;-6.64808111601071]
Robot Arm 1 Joint Angles: [-0.00246155428855998;1.86467445122021;-0.00246155428856644;-0.00729489198439593;-0.00246155428856644;-1.77880252411781;-0.00246155418807703]
Mobile Platform 2 Pose: [-2.3684748402874;9.97323744527938;6.40406251350065]
Robot Arm 2 Joint Angles: [0.00255270810340769;-2.04475627447462;0.00255270810337678;0.00566708752300952;0.00255270810341509;1.93569424248023;0.00255270697305714]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531312e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.436896e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.790044326148235;-9.97171662256931;-6.65793876118018]
Robot Arm 1 Joint Angles: [-0.00246155428856182;2.16962712076782;-0.00246155428856826;-0.00729489198439141;-0.00246155428856826;-2.08375519366543;-0.00246155418807886]
Mobile Platform 2 Pose: [-2.3684748402874;9.97277027498938;6.40352651708258]
Robot Arm 2 Joint Angles: [0.00255270810340957;-2.11950352087405;0.00255270810337866;0.005667087523004;0.00255270810341697;2.01044148887967;0.00255270697305902]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.612872e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.877425e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.790044326148235;-9.97330001737701;-6.6496824591071]
Robot Arm 1 Joint Angles: [-0.00246155428856288;1.91628395153492;-0.00246155428856933;-0.00729489198438896;-0.00246155428856933;-1.83041202443253;-0.00246155418807993]
Mobile Platform 2 Pose: [-2.3684748402874;9.97401021958075;6.40535939791686]
Robot Arm 2 Joint Angles: [0.00255270810341068;-1.92111238625529;0.00255270810337977;0.00566708752300081;0.00255270810341808;1.81205035426091;0.00255270697306013]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408491e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.708846e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.790044326148235;-9.97255494319421;-6.65373733956039]
Robot Arm 1 Joint Angles: [-0.00246155428856355;2.03549582078355;-0.00246155428857;-0.00729489198438731;-0.00246155428857;-1.94962389368117;-0.0024615541880806]
Mobile Platform 2 Pose: [-2.3684748402874;9.97217471768431;6.40249594390257]
Robot Arm 2 Joint Angles: [0.00255270810341139;-2.21479268968584;0.00255270810338048;0.00566708752299865;0.0025527081034188;2.10573065769147;0.00255270697306084]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394166e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.342311e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.790044326148235;-9.97234650880419;-6.65460289302438]
Robot Arm 1 Joint Angles: [-0.00246155428856396;2.06884532318639;-0.00246155428857041;-0.00729489198438633;-0.00246155428857041;-1.98297339608401;-0.002461554188081]
Mobile Platform 2 Pose: [-2.3684748402874;9.9741068451432;6.40560850935508]
Robot Arm 2 Joint Angles: [0.00255270810341179;-1.90565229626384;0.00255270810338088;0.00566708752299756;0.00255270810341919;1.79659026426947;0.00255270697306123]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956266e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.242285e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.790044326148235;-9.97343026740113;-6.64918970541419]
Robot Arm 1 Joint Angles: [-0.00246155428856419;1.89544394767589;-0.00246155428857065;-0.00729489198438581;-0.00246155428857065;-1.80957202057351;-0.00246155418808124]
Mobile Platform 2 Pose: [-2.3684748402874;9.97254621882616;6.40295503621742]
Robot Arm 2 Joint Angles: [0.00255270810341206;-2.15535250698999;0.00255270810338115;0.00566708752299672;0.00255270810341946;2.04629047499562;0.00255270697306151]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260397e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.128517e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.790044326148235;-9.97164968923927;-6.65826255544698]
Robot Arm 1 Joint Angles: [-0.00246155428856437;2.18033645357462;-0.00246155428857081;-0.00729489198438533;-0.00246155428857081;-2.09446452647224;-0.0024615541880814]
Mobile Platform 2 Pose: [-2.3684748402874;9.97348095804785;6.40462459093723]
Robot Arm 2 Joint Angles: [0.00255270810341221;-2.00579423151931;0.00255270810338129;0.00566708752299632;0.00255270810341961;1.89673219952495;0.00255270697306165]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807396e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.375972e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.790044326148235;-9.97358545580974;-6.64829084833196]
Robot Arm 1 Joint Angles: [-0.00246155428856443;1.87061380229857;-0.00246155428857089;-0.00729489198438524;-0.00246155428857089;-1.78474187519619;-0.00246155418808149]
Mobile Platform 2 Pose: [-2.3684748402874;9.97348936676556;6.40440908863466]
Robot Arm 2 Joint Angles: [0.0025527081034123;-2.0044488366857;0.00255270810338139;0.00566708752299605;0.0025527081034197;1.89538680469133;0.00255270697306174]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620237e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.735609e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.790044326148235;-9.97200013288736;-6.65659600493811]
Robot Arm 1 Joint Angles: [-0.00246155428856451;2.12426546987906;-0.00246155428857095;-0.00729489198438501;-0.00246155428857095;-2.03839354277669;-0.00246155418808155]
Mobile Platform 2 Pose: [-2.3684748402874;9.97253879155933;6.40308444863826]
Robot Arm 2 Joint Angles: [0.00255270810341236;-2.15654086968256;0.00255270810338145;0.00566708752299583;0.00255270810341976;2.04747883768819;0.00255270697306181]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640576e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.324701e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.790044326148235;-9.97292968851742;-6.65164362580475]
Robot Arm 1 Joint Angles: [-0.00246155428856453;1.97553656907075;-0.00246155428857098;-0.00729489198438498;-0.00246155428857098;-1.88966464196837;-0.00246155418808158]
Mobile Platform 2 Pose: [-2.3684748402874;9.97410943224639;6.40548949937395]
Robot Arm 2 Joint Angles: [0.00255270810341239;-1.90523835975225;0.00255270810338147;0.00566708752299581;0.00255270810341978;1.79617632775788;0.00255270697306183]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711081e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.414915e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.790044326148235;-9.9729918326042;-6.6515461636986]
Robot Arm 1 Joint Angles: [-0.00246155428856455;1.9655935151852;-0.002461554288571;-0.00729489198438494;-0.002461554288571;-1.87972158808282;-0.0024615541880816]
Mobile Platform 2 Pose: [-2.3684748402874;9.97217604761185;6.40240608397989]
Robot Arm 2 Joint Angles: [0.00255270810341242;-2.21457990127968;0.00255270810338151;0.00566708752299565;0.00255270810341982;2.10551786928532;0.00255270697306187]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373306e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.219989e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.790044326148235;-9.97194709847647;-6.65673594761773]
Robot Arm 1 Joint Angles: [-0.00246155428856458;2.1327509756219;-0.00246155428857102;-0.00729489198438484;-0.00246155428857102;-2.04687904851953;-0.00246155418808162]
Mobile Platform 2 Pose: [-2.3684748402874;9.97400582659337;6.4054004504545]
Robot Arm 2 Joint Angles: [0.00255270810341242;-1.92181526423612;0.00255270810338151;0.00566708752299571;0.00255270810341982;1.81275323224175;0.00255270697306187]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767665e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.878171e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.790044326148235;-9.97360123860836;-6.64831657444213]
Robot Arm 1 Joint Angles: [-0.00246155428856456;1.86808855451986;-0.00246155428857102;-0.00729489198438493;-0.00246155428857102;-1.78221662741748;-0.00246155418808162]
Mobile Platform 2 Pose: [-2.3684748402874;9.97277892504583;6.40323936775531]
Robot Arm 2 Joint Angles: [0.00255270810341244;-2.11811951184286;0.00255270810338153;0.00566708752299562;0.00255270810341984;2.0090574798485;0.00255270697306188]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002368e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.312326e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.790044326148235;-9.97166632779981;-6.65828906996351]
Robot Arm 1 Joint Angles: [-0.0024615542885646;2.17767428388823;-0.00246155428857103;-0.0072948919843848;-0.00246155428857103;-2.09180235678585;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.3684748402874;9.973228379882;6.40415750639605]
Robot Arm 2 Joint Angles: [0.00255270810341244;-2.04620673805496;0.00255270810338153;0.00566708752299563;0.00255270810341984;1.93714470606059;0.00255270697306189]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.186982e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.481662e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.790044326148235;-9.97339148630232;-6.64932545706367]
Robot Arm 1 Joint Angles: [-0.00246155428856457;1.90164892348567;-0.00246155428857103;-0.0072948919843849;-0.00246155428857103;-1.81577699638329;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.3684748402874;9.97371421849533;6.40472430872608]
Robot Arm 2 Joint Angles: [0.00255270810341244;-1.96847255992184;0.00255270810338153;0.00566708752299565;0.00255270810341984;1.85941052792747;0.00255270697306188]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546741e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.458554e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.790044326148235;-9.97241328267115;-6.65456959640857]
Robot Arm 1 Joint Angles: [-0.00246155428856459;2.05816150447293;-0.00246155428857104;-0.00729489198438483;-0.00246155428857104;-1.97228957737055;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.3684748402874;9.97235169124551;6.40270850809317]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.1864769198944;0.00255270810338154;0.00566708752299557;0.00255270810341985;2.07741488790003;0.0025527069730619]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.135958e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.509575e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.790044326148235;-9.97248703675984;-6.65398618943883]
Robot Arm 1 Joint Angles: [-0.00246155428856459;2.04636085028395;-0.00246155428857104;-0.00729489198438483;-0.00246155428857104;-1.96048892318157;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.3684748402874;9.97415580781511;6.40553325462371]
Robot Arm 2 Joint Angles: [0.00255270810341244;-1.89781826875844;0.00255270810338153;0.00566708752299566;0.00255270810341984;1.78875623676408;0.00255270697306188]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235399e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.137447e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.790044326148235;-9.97334546663012;-6.64975128279978]
Robot Arm 1 Joint Angles: [-0.00246155428856458;1.90901207103905;-0.00246155428857103;-0.00729489198438489;-0.00246155428857103;-1.82314014393667;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.3684748402874;9.97225212853036;6.40243237543862]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.202406954318;0.00255270810338154;0.00566708752299556;0.00255270810341985;2.09334492232364;0.0025527069730619]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.017688e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.047321e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.790044326148235;-9.97169001701826;-6.6581511812224]
Robot Arm 1 Joint Angles: [-0.0024615542885646;2.17388400893679;-0.00246155428857104;-0.00729489198438478;-0.00246155428857104;-2.08801208183441;-0.00246155418808164]
Mobile Platform 2 Pose: [-2.3684748402874;9.973854313211;6.40510561676153]
Robot Arm 2 Joint Angles: [0.00255270810341244;-1.94605740541585;0.00255270810338153;0.00566708752299564;0.00255270810341984;1.83699537342148;0.00255270697306189]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924111e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.892234e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.790044326148235;-9.97361436230358;-6.64826296636228]
Robot Arm 1 Joint Angles: [-0.00246155428856457;1.86598876328446;-0.00246155428857103;-0.0072948919843849;-0.00246155428857103;-1.78011683618208;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.3684748402874;9.97303625092442;6.40357269700526]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.07694737126837;0.00255270810338154;0.0056670875229956;0.00255270810341985;1.967885339274;0.00255270697306189]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213663e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.728664e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.790044326148235;-9.97189295741077;-6.65724047403901]
Robot Arm 1 Joint Angles: [-0.0024615542885646;2.14141354613467;-0.00246155428857104;-0.00729489198438479;-0.00246155428857104;-2.05554161903229;-0.00246155418808164]
Mobile Platform 2 Pose: [-2.36729653784912;9.97296594283595;6.4036720928134]
Robot Arm 2 Joint Angles: [0.0025527062407673;-2.08819666356075;0.00255270624073639;0.00566708938564075;0.0025527062407747;1.97913463156638;0.00255270511041674]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673203e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.879629e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.790044326148235;-9.97305747138642;-6.65109604981416]
Robot Arm 1 Joint Angles: [-0.00246155428856458;1.9550913100296;-0.00246155428857103;-0.00729489198438487;-0.00246155428857103;-1.86921938292722;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.36729653784912;9.97389971616938;6.40498592744632]
Robot Arm 2 Joint Angles: [0.00255270624076729;-1.93879293021154;0.00255270624073638;0.00566708938564079;0.00255270624077469;1.82973089821717;0.00255270511041674]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183017e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.412885e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.790044326148235;-9.97285991378528;-6.65233951444717]
Robot Arm 1 Joint Angles: [-0.00246155428856459;1.98670052621302;-0.00246155428857104;-0.00729489198438485;-0.00246155428857104;-1.90082859911064;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.36729653784912;9.97222462447827;6.40242300347125]
Robot Arm 2 Joint Angles: [0.0025527062407673;-2.20680760078958;0.00255270624073639;0.00566708938564071;0.00255270624077471;2.09774556879521;0.00255270511041675]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.344484e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.640650e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.790044326148235;-9.97206147084533;-6.65625014813564]
Robot Arm 1 Joint Angles: [-0.0024615542885646;2.11445139660514;-0.00246155428857104;-0.0072948919843848;-0.00246155428857104;-2.02857946950276;-0.00246155418808164]
Mobile Platform 2 Pose: [-2.36609056802215;9.97414861192556;6.40548666820483]
Robot Arm 2 Joint Angles: [0.00255270810341244;-2.02136636723222;0.00255270810338153;0.00566708752299566;0.00255270810341984;1.8032212574632;0.00255270697306188]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.852446e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.292170e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.786434344649049;-9.97356419659162;-6.64863363130451]
Robot Arm 1 Joint Angles: [-0.00246155615120973;1.92901016358074;-0.00246155615121618;-0.00729489012173975;-0.00246155615121618;-1.78814335195861;-0.00246155605072678]
Mobile Platform 2 Pose: [-2.36609056802215;9.97239111679943;6.40256178812874]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.30256558741264;0.00255270810338154;0.00566708752299557;0.00255270810341985;2.08442047764362;0.0025527069730619]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.136554e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.330795e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.786434344649049;-9.97163721468178;-6.65853098433682]
Robot Arm 1 Joint Angles: [-0.00246155615120975;2.23732726915418;-0.00246155615121619;-0.00729489012173963;-0.00246155615121619;-2.09646045753205;-0.00246155605072679]
Mobile Platform 2 Pose: [-2.36609056802215;9.97382266569336;6.40498322415205]
Robot Arm 2 Joint Angles: [0.00255270810341244;-2.07351776438301;0.00255270810338153;0.00566708752299564;0.00255270810341984;1.85537265461398;0.00255270697306189]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.628714e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.605182e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.786434344649049;-9.97350063080027;-6.64888824877249]
Robot Arm 1 Joint Angles: [-0.00246155615120973;1.93918069019705;-0.00246155615121618;-0.00729489012173974;-0.00246155615121618;-1.79831387857493;-0.00246155605072678]
Mobile Platform 2 Pose: [-2.36609056802215;9.97304684349677;6.40351987316122]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.19764931583858;0.00255270810338154;0.0056670875229956;0.00255270810341985;1.97950420606955;0.00255270697306189]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.036082e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.326896e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.782887774461308;-9.97215008871829;-6.65601767134827]
Robot Arm 1 Joint Angles: [-0.0024615542885646;2.0943951023932;-0.00246155428857104;-0.00729489198438481;-0.00246155428857104;-2.01440060982837;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.36609056802215;9.97315276383978;6.40389897741244]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.18070206095551;0.00255270810338154;0.00566708752299561;0.00255270810341985;1.96255695118648;0.00255270697306189]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.198965e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.638360e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.782887774461308;-9.97282571329452;-6.65235658985621]
Robot Arm 1 Joint Angles: [-0.00246155428856459;1.98629517019574;-0.00246155428857104;-0.00729489198438485;-0.00246155428857104;-1.90630067763091;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.36609056802215;9.97374578250747;6.40464682069362]
Robot Arm 2 Joint Angles: [0.00255270810341244;-2.08581907412509;0.00255270810338153;0.00566708752299564;0.00255270810341984;1.86767396435606;0.00255270697306189]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.165569e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.262072e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.782887774461308;-9.97316086351983;-6.65084639158364]
Robot Arm 1 Joint Angles: [-0.00246155428856458;1.93267113414628;-0.00246155428857103;-0.00729489198438487;-0.00246155428857103;-1.85267664158145;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.36609056802215;9.97246196109812;6.4027647566006]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.29123049962193;0.00255270810338154;0.00566708752299557;0.00255270810341985;2.0730853898529;0.0025527069730619]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.370049e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.180880e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.782887774461308;-9.97186790023173;-6.65733141712152]
Robot Arm 1 Joint Angles: [-0.0024615542885646;2.13954526024219;-0.00246155428857104;-0.00729489198438479;-0.00246155428857104;-2.05955076767736;-0.00246155418808164]
Mobile Platform 2 Pose: [-2.36609056802215;9.97412783086854;6.40533991615606]
Robot Arm 2 Joint Angles: [0.00255270810341244;-2.02469133635486;0.00255270810338153;0.00566708752299565;0.00255270810341984;1.80654622658583;0.00255270697306188]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.051306e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.369957e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.782887774461308;-9.97366189636836;-6.64816401288165]
Robot Arm 1 Joint Angles: [-0.00246155428856457;1.85250587838185;-0.00246155428857103;-0.0072948919843849;-0.00246155428857103;-1.77251138581702;-0.00246155418808163]
Mobile Platform 2 Pose: [-2.36609056802215;9.97219827804869;6.40226865660212]
Robot Arm 2 Joint Angles: [0.00255270810341245;-2.33341978753021;0.00255270810338154;0.00566708752299556;0.00255270810341986;2.11527467776119;0.0025527069730619]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.127251e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.785019e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,1)">15  </a>    translation_norm = norm(s(4:end));
inv(a)
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.785019e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 15)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',15,0)">第 15 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

ans =

   1.0e+18 *

    0.0000   -0.0000    0.0000    9.3372
    0.0000    0.0000   -0.0000   -0.0106
   -0.0000    0.0000    0.0000   -1.8621
         0         0         0    0.0000

if system_dependent('IsDebugMode')==1, dbcont; end
mobile_robot.car.J*X(1:4)

ans =

   -0.0000
    0.0076
    0.0123

s_d

s_d =

    0.0000
    0.0000
    0.0000
    0.9806
    0.0123
   -0.1956

if system_dependent('IsDebugMode')==1, dbquit all; end
mobile_robot.car.J*X(1:4)
{无法解析名称 'mobile_robot.car.J'。
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',73,1)">73  </a>    dx = fmincon(fun1, X, [], [], [], [], [], [], fun2, options);
mobile_robot.car.J*X(1:4)

ans =

   -0.0000
    0.0056
    0.6875

mobile_robot.arm.J_body*X(5:end)

ans =

         0
    0.0000
   -0.0019
    0.7206
         0
         0

s_d

s_d =

         0
         0
   -0.0019
    0.7262
    0.6875
         0

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,1)">85  </a>    [dx1] = mobile_robot_CBLF(mobile_robot1, target1, a1_G, obstacle_results1, D_min, V_max, dt);
if system_dependent('IsDebugMode')==1, dbcont; end

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 Joint Angles: [-0.000121114227682437;0.185441233024398;-0.000121114227682768;1.86264489701995e-09;-0.000121114227682768;-0.225185848658087;-0.000121114227682768]
Mobile Platform 2 Pose: [-2.35619437191491;9.99866771753545;9.81844390937036]
Robot Arm 2 Joint Angles: [0.000121114227682424;-0.185441233024398;0.000121114227682774;-1.86264487453752e-09;0.000121114227682774;0.213165194053599;0.000121114227682774]
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/human/update_human_models.m',42,1)">42  </a>        human_models(i).pose = pose; % 更新位姿
pose = [planar_poses(:, i)]

pose =

   -9.5752
   -9.5752
         0

pose = planar_poses(:, i)

pose =

   -9.5752
   -9.5752
         0

transform_matrix = get_Tc(pose)

transform_matrix =

    0.9861    0.1663         0   -9.5752
   -0.1663    0.9861         0         0
         0         0    1.0000         0
         0         0         0    1.0000

planar_poses

planar_poses =

   -9.5752    2.9242    0.0375
   -9.5752    2.7293    1.3647
         0         0         0

transform_matrix = get_Tc(pose)

transform_matrix =

    0.9861    0.1663         0   -9.5752
   -0.1663    0.9861         0         0
         0         0    1.0000         0
         0         0         0    1.0000

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.449150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 Joint Angles: [-0.000121114227683022;0.185441233024398;-0.000121114227681381;1.86264489701995e-09;-0.000121114227682768;-0.22518584865809;-0.00012111422768375]
Mobile Platform 2 Pose: [-2.35619437191491;9.99866771753545;9.81844390937036]
Robot Arm 2 Joint Angles: [0.000121114227682424;-0.185441233024398;0.000121114227682774;-1.86264487453752e-09;0.000121114227682774;0.213165194053599;0.000121114227682774]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.934576e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 Joint Angles: [-0.000242257227143282;0.370882466048795;-0.00024225722714405;-0.000118135763669368;-0.000242257227146825;-0.450777934956321;-0.000242257227154676]
Mobile Platform 2 Pose: [-2.35619425360615;9.99733167719312;9.63690106041641]
Robot Arm 2 Joint Angles: [0.000242260535616737;-0.370882466048795;0.000242260535617437;8.24406669025269e-05;0.000242260535617437;0.426849204164602;0.000242260535617437]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.610340e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 Joint Angles: [-0.000363286279799942;0.556323699073193;-0.000363286279798916;-0.000357282809229051;-0.000363286279802673;-0.676844213293337;-0.000363286279809542]
Mobile Platform 2 Pose: [-2.35619413529215;9.99599115574577;9.45545454589275]
Robot Arm 2 Joint Angles: [0.000363412210349703;-0.556323699073193;0.000363412210350752;0.000250017184069795;0.000363412210350752;0.641165057095127;0.000363412210350752]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.441059e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 Joint Angles: [-0.000483866411480727;0.74176493209759;-0.000483866411478066;-0.000719510076232192;-0.000483866411482804;-0.903917115513842;-0.000483866411488692]
Mobile Platform 2 Pose: [-2.35619401716568;9.99464273095435;9.27451030558023]
Robot Arm 2 Joint Angles: [0.000484371855473429;-0.74176493209759;0.000484371855474827;0.000505156626171682;0.000484371855474827;0.856657882162591;0.000484371855474827]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.401603e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 Joint Angles: [-0.0006036745926073;0.927206165121988;-0.000603674592606945;-0.00120731922081321;-0.000603674592618552;-1.13251733087994;-0.000603674592617571]
Mobile Platform 2 Pose: [-2.35619389940294;9.99328288341341;9.09451230889671]
Robot Arm 2 Joint Angles: [0.000604959038630923;-0.927206165121988;0.000604959038632667;0.000851202889335113;0.000604959038632667;1.07388744034533;0.000604959038632667]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.474601e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346623]
Robot Arm 1 Joint Angles: [-0.000722796083224335;1.11264739814639;-0.00072279608322361;-0.00182553790636024;-0.000722796083242086;-1.36238137284088;-0.000722796083234236]
Mobile Platform 2 Pose: [-2.35619378181687;9.99191235316415;8.91540999400951]
Robot Arm 2 Joint Angles: [0.000725365317144262;-1.11264739814639;0.000725365317146352;0.00129364656158718;0.000725365317146352;1.29272983446354;0.000725365317146352]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528709;-9.99002831581144;-8.79949883355178]
Robot Arm 1 Joint Angles: [-0.000841580556365119;1.29808863117078;-0.000841580556358477;-0.00257974514714843;-0.000841580556377935;-1.59288971083287;-0.000841580556369103]
Mobile Platform 2 Pose: [-2.3559563562191;9.99053427700863;8.73682165392071]
Robot Arm 2 Joint Angles: [0.000846034930370361;-1.29808863117078;0.000846034930372872;0.00183861009437647;0.000846034930372872;1.51267720998909;0.000846034930372872]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274173]
Robot Arm 1 Joint Angles: [-0.000959829987060112;1.48352986419518;-0.000959829987047145;-0.00347025803777765;-0.000959829987065621;-1.82450740028498;-0.00095982998705679]
Mobile Platform 2 Pose: [-2.3557253005368;9.98914653838652;8.55900681080801]
Robot Arm 2 Joint Angles: [0.000966974256339863;-1.48352986419518;0.000966974256342699;0.00248885218009608;0.000966974256342699;1.73406530168165;0.000966974256342699]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196129;-9.98711511781;-8.46062525935787]
Robot Arm 1 Joint Angles: [-0.00107743739451576;1.66897109721958;-0.0010774373945052;-0.00449854422257743;-0.00107743739452368;-2.05708258800973;-0.00107743739451485]
Mobile Platform 2 Pose: [-2.35551241397406;9.98774922249154;8.38210760212881]
Robot Arm 2 Joint Angles: [0.00108827693604603;-1.66897109721958;0.00108827693604926;0.00324802395508969;0.00108827693604926;1.95687649526838;0.00108827693604926]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929674;-9.98565931998688;-8.29072857539468]
Robot Arm 1 Joint Angles: [-0.00119546838174874;1.85441233024398;-0.00119546838174059;-0.00567665166884611;-0.00119546838175906;-2.0943951023932;-0.00119546838175023]
Mobile Platform 2 Pose: [-2.35536693013457;9.98635046198854;8.20503665489848]
Robot Arm 2 Joint Angles: [0.00121089004730276;-1.85441233024398;0.00121089004730053;0.00412547674574894;0.0012108900473063;2.0943951023932;0.0012108900473063]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.9841823282427;-8.12462828169497]
Robot Arm 1 Joint Angles: [-0.00131696123650541;2.03985356326837;-0.00131696123649832;-0.00639922083539073;-0.00131696123651679;-2.30399143679527;-0.00131696052350609]
Mobile Platform 2 Pose: [-2.35536680994485;9.98493696485757;8.03017066322552]
Robot Arm 2 Joint Angles: [0.00133396239245027;-2.03985356326837;0.00133396239244748;0.00485304264132445;0.00133396239245414;2.29681439990543;0.00133396239245414]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840344088;-7.9599482888563]
Robot Arm 1 Joint Angles: [-0.00143870942785493;2.14949927567626;-0.00143870942784766;-0.00719336673714352;-0.00143870942786614;-2.22826414941097;-0.00143870871485544]
Mobile Platform 2 Pose: [-2.35536668873674;9.98351834838216;7.85623335939152]
Robot Arm 2 Joint Angles: [0.00145807819417089;-2.14636015252078;0.00145807819416362;0.00563277697259847;0.0014580781941751;2.22298056213083;0.0014580781941751]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455234098;-7.79314279536571]
Robot Arm 1 Joint Angles: [-0.00156630119507468;2.18077941225243;-0.00156630119506725;-0.00742071448357543;-0.00156630119508573;-2.20427246494267;-0.00156630048207503]
Mobile Platform 2 Pose: [-2.35536656497315;9.98211891582933;7.6819256141284]
Robot Arm 2 Joint Angles: [0.00158481080822112;-2.17722464642395;0.00158481080820938;0.005856996022263;0.00158481080822567;2.20007623709485;0.00158481080822567]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.9797235609508;-7.63794018922978]
Robot Arm 1 Joint Angles: [-0.00168692949029622;2.19333238380943;-0.00168692949028909;-0.00746871767647478;-0.00168692949030756;-2.20035149691845;-0.00168692877729686]
Mobile Platform 2 Pose: [-2.35536644599929;9.98066880808683;7.51575684757177]
Robot Arm 2 Joint Angles: [0.00170663873677138;-2.18886460764673;0.00170663873675515;0.00590856152375278;0.00170663873677625;2.19568914476479;0.00170663873677625]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591665116;-7.46636918684545]
Robot Arm 1 Joint Angles: [-0.00182021652145274;2.1916426546638;-0.00182021652144546;-0.00746685601467987;-0.00182021652146393;-2.1937495063759;-0.00182021580845323]
Mobile Platform 2 Pose: [-2.35536631763683;9.97930447849414;7.33822023584897]
Robot Arm 2 Joint Angles: [0.00183808058576142;-2.18822464182846;0.00183808058574073;0.00591145044819667;0.00183808058576663;2.19027026882691;0.00183808058576663]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346212;-9.97678818484225;-7.31817266015123]
Robot Arm 1 Joint Angles: [-0.00193743410109527;2.19689567720951;-0.00193743410108827;-0.0074507970691896;-0.00193743410110675;-2.19753736887274;-0.00193743338809605]
Mobile Platform 2 Pose: [-2.35536620176429;9.97782827593779;7.17879822025303]
Robot Arm 2 Joint Angles: [0.00195673277140284;-2.19339820677208;0.00195673277137764;0.00589984999877262;0.00195673277140835;2.19401885592357;0.00195673277140835]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232725;-9.9753590119579;-7.16418511934567]
Robot Arm 1 Joint Angles: [-0.00205940500907929;2.19373921813631;-0.00205940500907258;-0.00742961334509667;-0.00205940500909106;-2.19394443665204;-0.00205940429608036]
Mobile Platform 2 Pose: [-2.3553660779242;9.97645066542148;7.01004954653045]
Robot Arm 2 Joint Angles: [0.0020835437206921;-2.19021626245171;0.00208354372066242;0.00588293002548733;0.00208354372069794;2.19041260996344;0.00208354372069794]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.78539602676002;-9.97430990779222;-7.00084677780109]
Robot Arm 1 Joint Angles: [-0.00218788775603952;2.1745482189954;-0.00218788775603313;-0.00740588686305206;-0.00218788775605161;-2.17462402122166;-0.00218788704304091]
Mobile Platform 2 Pose: [-2.35536595347757;9.97523765871612;6.84194328439722]
Robot Arm 2 Joint Angles: [0.00221097576495081;-2.18136026626063;0.00221097576491665;0.00586457695925152;0.00221097576495698;2.1814301831362;0.00221097576495698]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.78539595660333;-9.97259808478388;-6.91581468798367]
Robot Arm 1 Joint Angles: [-0.00225972690260118;2.20060448127733;-0.00225972690259496;-0.00739236789516434;-0.00225972690261343;-2.20063535217658;-0.00225972618960273]
Mobile Platform 2 Pose: [-2.35536587032776;9.97361874427101;6.72918800357383]
Robot Arm 2 Joint Angles: [0.00229611986235293;-2.19818877855304;0.00229611986231899;0.00585204495466653;0.00229611986235931;2.19821729879238;0.00229611986235931]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227853;-9.97409553094106;-6.77330520679708]
Robot Arm 1 Joint Angles: [-0.00236353352892908;2.01516324825293;-0.00236353352892313;-0.00737275824162398;-0.0023635335289416;-1.96106357862124;-0.0023635328159309]
Mobile Platform 2 Pose: [-2.35536572876772;9.97457563652786;6.54430712143887]
Robot Arm 2 Joint Angles: [0.00244107735126521;-2.04506456624343;0.00244107735123163;0.00583059374161259;0.00244107735127195;2.04513598890888;0.00244107735127195]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.97228129699828;-6.73833367577277]
Robot Arm 1 Joint Angles: [-0.0023973975264219;2.09439509667864;-0.00239739752641602;-0.00731935067018818;-0.00239739752643449;-2.09439509417083;-0.00239739681342379]
Mobile Platform 2 Pose: [-2.35536568714707;9.97272052613738;6.48632568145895]
Robot Arm 2 Joint Angles: [0.00248369558783403;-2.16863934793181;0.00248369558780056;0.00582426017514512;0.00248369558784088;2.16866457346442;0.00248369558784088]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540074;-9.97333331315079;-6.69737167009219]
Robot Arm 1 Joint Angles: [-0.00242465195277759;1.92606732656943;-0.00242465195277178;-0.00731416496329334;-0.00242465195279026;-1.9260776954754;-0.00242465123977956]
Mobile Platform 2 Pose: [-2.35536564926464;9.9739618095472;6.43903065411302]
Robot Arm 2 Joint Angles: [0.00252248533295088;-1.98319811490742;0.0025224853329175;0.00581851145556026;0.00252248533295782;1.97006497885838;0.00252248533295782]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395781195851;-9.97294909644746;-6.68023583759776]
Robot Arm 1 Joint Angles: [-0.00243934043716969;1.98753916361096;-0.00243934043716392;-0.00731132947224667;-0.0024393404371824;-1.98755520349902;-0.0024393397241717]
Mobile Platform 2 Pose: [-2.35536563071905;9.97323100305053;6.4131577730182]
Robot Arm 2 Joint Angles: [0.00254147601741022;-2.10011785854618;0.00254147601737689;0.00580921562598943;0.00254147601741721;2.08700331415629;0.00254147601741721]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395772638942;-9.97200303864331;-6.67347995257358]
Robot Arm 1 Joint Angles: [-0.00244810084900341;2.0943951023932;-0.00244810084899765;-0.00730965080542708;-0.00244810084901613;-2.0943951023932;-0.00244810013600543]
Mobile Platform 2 Pose: [-2.35536561955056;9.97266608314766;6.39782993134891]
Robot Arm 2 Joint Angles: [0.00255291069116384;-2.0943951023932;0.00255291069113053;0.00580368210574136;0.00255291069117086;2.0943951023932;0.00255291069117086]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395767401621;-9.97358488642586;-6.65848980933812]
Robot Arm 1 Joint Angles: [-0.00245346200287458;1.90895386925231;-0.00245346200286885;-0.00730863212378707;-0.00245346200288733;-1.84130047961619;-0.00245346128987663]
Mobile Platform 2 Pose: [-2.35536561282259;9.97391774849087;6.39090529220584]
Robot Arm 2 Joint Angles: [0.00255979826604848;-1.9089538693688;0.00255979826601518;0.00580265385586007;0.00255979826605551;1.89412967945479;0.00255979826605551]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395764311851;-9.9716500757212;-6.66429501601706]
Robot Arm 1 Joint Angles: [-0.00245662406489679;2.0943950978115;-0.00245662406489105;-0.00730248004082667;-0.00245662406490952;-2.09439510583112;-0.00245662335189882]
Mobile Platform 2 Pose: [-2.35536560894474;9.97202466247249;6.38277075915936]
Robot Arm 2 Joint Angles: [0.00256376731958225;-2.09439510211619;0.00256376731954898;0.00580052417038857;0.0025637673195893;2.09439509537772;0.0025637673195893]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785395762440954;-9.97334587567019;-6.65299730614471]
Robot Arm 1 Joint Angles: [-0.00245853800054772;1.90895386479396;-0.002458538000542;-0.00730211760364607;-0.00245853800056048;-1.82306748031366;-0.00245853728754978]
Mobile Platform 2 Pose: [-2.35536560657993;9.97388306649781;6.38271435081922]
Robot Arm 2 Joint Angles: [0.00256618702552555;-1.90895386948481;0.00256618702549228;0.0058001640657291;0.0025661870255326;1.79987078410193;0.0025661870255326]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785395761295571;-9.9725430019079;-6.65580126947701]
Robot Arm 1 Joint Angles: [-0.00245971087207978;2.03741082995867;-0.00245971087207406;-0.0072992808023168;-0.00245971087209253;-1.95153011908103;-0.00245971015908183]
Mobile Platform 2 Pose: [-2.35536560512677;9.97274363741051;6.3787507845464]
Robot Arm 2 Joint Angles: [0.00256767505856429;-2.09125808748428;0.00256767505853102;0.0057957280972727;0.00256767505857134;1.98218387403832;0.00256767505857134]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395760593853;-9.97237811570277;-6.65550571971024]
Robot Arm 1 Joint Angles: [-0.0024604294316762;2.06379090445736;-0.00246042943167048;-0.00729756247915696;-0.00246042943168896;-1.97791363022604;-0.00246042871867826]
Mobile Platform 2 Pose: [-2.35536560423752;9.97303658165993;6.37826501198608]
Robot Arm 2 Joint Angles: [0.00256858565921436;-2.0443843373892;0.00256858565918109;0.00579305790816589;0.00256858565922142;1.93531546432146;0.00256858565922142]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395760172427;-9.97341831965915;-6.64975907402211]
Robot Arm 1 Joint Angles: [-0.00246086097205636;1.89735724181491;-0.00246086097205065;-0.00729653285710986;-0.00246086097206912;-1.81148202682769;-0.00246086025905842]
Mobile Platform 2 Pose: [-2.35536560370006;9.97366097100638;6.37835068720794]
Robot Arm 2 Joint Angles: [0.002569136011381;-1.94448042165241;0.00256913601134773;0.00579143760379905;0.00256913601138806;1.8354147891934;0.00256913601138806]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395759909198;-9.97165700587365;-6.65837171221346]
Robot Arm 1 Joint Angles: [-0.00246113051844891;2.17916679493719;-0.00246113051844318;-0.00729588029970588;-0.00246113051846165;-2.09329288506478;-0.00246112980545095]
Mobile Platform 2 Pose: [-2.35536560336552;9.97217872885157;6.37569029133955]
Robot Arm 2 Joint Angles: [0.00256947858049288;-2.18163814873343;0.00256947858045962;0.00579041991530329;0.00256947858049995;2.07257455165141;0.00256947858049995]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395759753156;-9.97359339940187;-6.64819520364775]
Robot Arm 1 Joint Angles: [-0.00246129030556191;1.869343452737;-0.0024612903055562;-0.00729550261551014;-0.00246129030557468;-1.78347029823298;-0.00246128959256398]
Mobile Platform 2 Pose: [-2.35536560316658;9.97404000755082;6.37836205372206]
Robot Arm 2 Joint Angles: [0.00256968230004101;-1.88383296399383;0.00256968230000774;0.00578982705605427;0.00256968230004806;1.77477055263031;0.00256968230004806]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785395759655636;-9.97198118931604;-6.65650177558254]
Robot Arm 1 Joint Angles: [-0.00246139016627963;2.12729682409647;-0.0024613901662739;-0.0072952602415042;-0.00246139016629238;-2.04142415434046;-0.00246138945328168]
Mobile Platform 2 Pose: [-2.35536560304186;9.97218131291086;6.37514315097927]
Robot Arm 2 Joint Angles: [0.00256981001025507;-2.18122372483046;0.00256981001022181;0.00578944549923316;0.00256981001026214;2.07216207658058;0.00256981001026214]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395759597111;-9.97295374746973;-6.65125334903681]
Robot Arm 1 Joint Angles: [-0.00246145009563331;1.97168737727154;-0.00246145009562759;-0.00729511800759162;-0.00246145009564607;-1.88581499198336;-0.00246144938263537]
Mobile Platform 2 Pose: [-2.35536560296738;9.97365469368864;6.37753132264745]
Robot Arm 2 Joint Angles: [0.00256988628356397;-1.94548257841464;0.0025698862835307;0.00578922352773379;0.00256988628357103;1.83642137410776;0.00256988628357103]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785395759561427;-9.97296922404441;-6.6513492560343]
Robot Arm 1 Joint Angles: [-0.00246148663562581;1.96921103745212;-0.0024614866356201;-0.00729503013686692;-0.00246148663563858;-1.88333882790539;-0.00246148592262787]
Mobile Platform 2 Pose: [-2.35536560292147;9.97303298856935;6.37624161078162]
Robot Arm 2 Joint Angles: [0.00256993329005604;-2.04495525787493;0.00256993329002278;0.00578908390147471;0.0025699332900631;1.93589433282057;0.0025699332900631]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785395759539366;-9.97196650325642;-6.6562907865685]
Robot Arm 1 Joint Angles: [-0.00246150922616748;2.1296463091937;-0.00246150922616176;-0.00729497579976558;-0.00246150922618023;-2.04377420832117;-0.00246150851316953]
Mobile Platform 2 Pose: [-2.35536560289335;9.97272078659858;6.37592260073938]
Robot Arm 2 Joint Angles: [0.00256996208360579;-2.09490748843727;0.00256996208357252;0.00578899914036696;0.00256996208361285;1.98584673290512;0.00256996208361285]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.78539575952626;-9.97359580921727;-6.64798611087732]
Robot Arm 1 Joint Angles: [-0.00246152264733956;1.86895732361144;-0.00246152264733385;-0.0072949439527453;-0.00246152264735233;-1.78308528643295;-0.00246152193434162]
Mobile Platform 2 Pose: [-2.35536560287649;9.97385393393258;6.37754660945214]
Robot Arm 2 Joint Angles: [0.00256997934697241;-1.91360386439716;0.00256997934693914;0.00578894854073763;0.00256997934697947;1.80454321006427;0.00256997934697947]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785395759518025;-9.97165958807069;-6.65794808034011]
Robot Arm 1 Joint Angles: [-0.00246153108011986;2.178752686596;-0.00246153108011414;-0.00729492348487617;-0.00246153108013261;-2.09288069035325;-0.00246153036712191]
Mobile Platform 2 Pose: [-2.35536560286593;9.97206341122619;6.37476009296605]
Robot Arm 2 Joint Angles: [0.00256999016769731;-2.20008746518295;0.00256999016766405;0.00578891630349581;0.00256999016770437;2.09102687532454;0.00256999016770437]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785395759513141;-9.97340627578782;-6.64887127655015]
Robot Arm 1 Joint Angles: [-0.00246153608088411;1.89928264003497;-0.0024615360808784;-0.00729491166461823;-0.00246153608089688;-1.81341066743274;-0.00246153536788618]
Mobile Platform 2 Pose: [-2.35536560285966;9.97400093495202;6.37786043548784]
Robot Arm 2 Joint Angles: [0.00256999657944669;-1.89008365041844;0.00256999657941342;0.00578889767212662;0.00256999657945375;1.78102309782277;0.00256999657945375]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785395759510112;-9.97238834370791;-6.65431341078414]
Robot Arm 1 Joint Angles: [-0.00246153918221748;2.06215176531351;-0.00246153918221176;-0.00729490415802862;-0.00246153918223023;-1.97627980772445;-0.00246153846921953]
Mobile Platform 2 Pose: [-2.35536560285575;9.97238362436418;6.37512008903274]
Robot Arm 2 Joint Angles: [0.00257000059271222;-2.1488533324891;0.00257000059267896;0.0057888856891136;0.00257000059271928;2.03979280385946;0.00257000059271928]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785395759508273;-9.97251220481971;-6.6534707095392]
Robot Arm 1 Joint Angles: [-0.00246154106582999;2.04233398293087;-0.00246154106582428;-0.00729489966353731;-0.00246154106584275;-1.9564620343308;-0.00246154035283205]
Mobile Platform 2 Pose: [-2.35536560285339;9.97339872017989;6.37691196413879]
Robot Arm 2 Joint Angles: [0.00257000300265167;-1.98643799494388;0.00257000300261841;0.00578887865731719;0.00257000300265873;1.87737748037783;0.00257000300265873]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785395759507166;-9.97332888203844;-6.64945225394588]
Robot Arm 1 Joint Angles: [-0.00246154219933972;1.91166562522433;-0.00246154219933401;-0.00729489695433561;-0.00246154219935249;-1.82579368204266;-0.00246154148634179]
Mobile Platform 2 Pose: [-2.35536560285195;9.97333262346598;6.37656899117743]
Robot Arm 2 Joint Angles: [0.0025700044753575;-1.99701346481229;0.00257000447532423;0.00578887429930112;0.00257000447536456;1.88795295896228;0.00257000447536456]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785395759506474;-9.97169930686996;-6.65771324223667]
Robot Arm 1 Joint Angles: [-0.00246154290748153;2.1723976504694;-0.00246154290747581;-0.00729489524224964;-0.00246154290749428;-2.08652571071191;-0.00246154219448358]
Mobile Platform 2 Pose: [-2.35536560285106;9.97244145529389;6.37534623665104]
Robot Arm 2 Joint Angles: [0.00257000538543276;-2.1396003696552;0.0025700053853995;0.00578887160879603;0.00257000538543982;2.0305398691862;0.00257000538543982]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785395759506065;-9.97361778204734;-6.64785910460435]
Robot Arm 1 Joint Angles: [-0.00246154332633321;1.86544162109776;-0.0024615433263275;-0.00729489425172447;-0.00246154332634598;-1.77956968332132;-0.00246154261333528]
Mobile Platform 2 Pose: [-2.35536560285054;9.97398146575788;6.37769254601938]
Robot Arm 2 Joint Angles: [0.00257000592766322;-1.89319869383458;0.00257000592762995;0.00578887002545308;0.00257000592767028;1.78413819653225;0.00257000592767028]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785395759505809;-9.97187540409097;-6.656938911363]
Robot Arm 1 Joint Angles: [-0.00246154358894332;2.14422209348063;-0.00246154358893759;-0.00729489361411645;-0.00246154358895607;-2.05835015697939;-0.00246154287594537]
Mobile Platform 2 Pose: [-2.3553656028502;9.97205119520599;6.3746214903411]
Robot Arm 2 Joint Angles: [0.0025700062684109;-2.20204198111951;0.00257000626837763;0.005788869008333;0.00257000626841796;2.09298148585142;0.00257000626841796]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785395759505656;-9.97307917302321;-6.65059553927168]
Robot Arm 1 Joint Angles: [-0.00246154374588661;1.95161906394996;-0.00246154374588089;-0.00729489324214186;-0.00246154374589937;-1.86574712819268;-0.00246154303288867]
Mobile Platform 2 Pose: [-2.35536560285001;9.97389000532125;6.3776267776416]
Robot Arm 2 Joint Angles: [0.00257000647060523;-1.90783236209077;0.00257000647057196;0.00578886842088453;0.00257000647061229;1.79877186799759;0.00257000647061229]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785395759505562;-9.97283566172529;-6.65207568829744]
Robot Arm 1 Joint Angles: [-0.00246154384200664;1.99058087138579;-0.00246154384200093;-0.00729489301058375;-0.0024615438420194;-1.90470893609162;-0.0024615431290087]
Mobile Platform 2 Pose: [-2.35536560284988;9.97265176315255;6.37544582639906]
Robot Arm 2 Joint Angles: [0.00257000659646562;-2.10595110870682;0.00257000659643236;0.00578886804571599;0.00257000659647268;1.99689061536397;0.00257000659647268]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036872e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785395759505504;-9.97208304294275;-6.65574885789258]
Robot Arm 1 Joint Angles: [-0.00246154390119389;2.11099987644941;-0.00246154390118817;-0.00729489286848789;-0.00246154390120664;-2.02512794143944;-0.00246154318819594]
Mobile Platform 2 Pose: [-2.35536560284981;9.97310427150968;6.37637219885168]
Robot Arm 2 Joint Angles: [0.00257000667266799;-2.03354977134353;0.00257000667263473;0.00578886782257371;0.00257000667267505;1.92448927844697;0.00257000667267505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.811171e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.460197e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.78539575950547;-9.97355589919412;-6.64829011432792]
Robot Arm 1 Joint Angles: [-0.00246154393643776;1.87534287614746;-0.00246154393643205;-0.00729489278473175;-0.00246154393645053;-1.78947094130499;-0.00246154322343983]
Mobile Platform 2 Pose: [-2.35536560284976;9.97359870831738;6.37694808935617]
Robot Arm 2 Joint Angles: [0.00257000671881659;-1.95443988197444;0.00257000671878332;0.0057888676865874;0.00257000671882365;1.84537938934985;0.00257000671882365]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563931e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.945060e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785395759505448;-9.97163424244322;-6.65815397495836]
Robot Arm 1 Joint Angles: [-0.0024615439585739;2.18280795623652;-0.00246154395856817;-0.00729489273103497;-0.00246154395858665;-2.09693602150146;-0.00246154324557594]
Mobile Platform 2 Pose: [-2.35536560284973;9.97222044996811;6.37491024192454]
Robot Arm 2 Joint Angles: [0.00257000674755394;-2.17496121777304;0.00257000674752068;0.00578886760129835;0.002570006747561;2.06590072531903;0.002570006747561]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056079e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.521890e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785395759505435;-9.97348102378628;-6.64859180451582]
Robot Arm 1 Joint Angles: [-0.00246154397168094;1.88732294131718;-0.00246154397167523;-0.0072948927000638;-0.00246154397169371;-1.80145100664405;-0.00246154325868301]
Mobile Platform 2 Pose: [-2.35536560284972;9.97403641082237;6.37775362246457]
Robot Arm 2 Joint Angles: [0.00257000676460747;-1.8844074810413;0.0025700067645742;0.00578886755164462;0.00257000676461453;1.77534698868659;0.00257000676461453]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231848e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.355116e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785395759505427;-9.97225080614539;-6.65511445526811]
Robot Arm 1 Joint Angles: [-0.00246154397983688;2.08415776383889;-0.00246154397983116;-0.00729489268030123;-0.00246154397984964;-1.99828582920529;-0.00246154326683894]
Mobile Platform 2 Pose: [-2.35536560284971;9.97214185114981;6.37466240519313]
Robot Arm 2 Joint Angles: [0.0025700067753226;-2.18753702861911;0.00257000677528934;0.0057888675196335;0.00257000677532966;2.07847653632843;0.00257000677532966]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531990e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.933008e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785395759505423;-9.97265434030515;-6.65284295629283]
Robot Arm 1 Joint Angles: [-0.00246154398476928;2.01959229826642;-0.00246154398476357;-0.00729489266855363;-0.00246154398478204;-1.93372036365632;-0.00246154327177134]
Mobile Platform 2 Pose: [-2.3553656028497;9.97370946111992;6.37728573610822]
Robot Arm 2 Joint Angles: [0.0025700067817057;-1.93671943338352;0.00257000678167244;0.00578886750106696;0.00257000678171276;1.82765894112997;0.00257000678171276]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692260e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.293405e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.78539575950542;-9.97322813797382;-6.65009145689996]
Robot Arm 1 Joint Angles: [-0.00246154398774938;1.92778467127224;-0.00246154398774367;-0.00729489266141704;-0.00246154398776215;-1.84191273667641;-0.00246154327475145]
Mobile Platform 2 Pose: [-2.3553656028497;9.97295437796401;6.37584742058296]
Robot Arm 2 Joint Angles: [0.00257000678564954;-2.05753273831623;0.00257000678561627;0.00578886748934286;0.0025700067856566;1.94847224608613;0.0025700067856566]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663859e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.912598e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.785395759505418;-9.97176377613675;-6.65747813759201]
Robot Arm 1 Joint Angles: [-0.00246154398960598;2.16208256519771;-0.00246154398960025;-0.00729489265693481;-0.00246154398961873;-2.07621063061084;-0.00246154327660803]
Mobile Platform 2 Pose: [-2.3553656028497;9.97279673532678;6.37581223041285]
Robot Arm 2 Joint Angles: [0.00257000678805931;-2.08275556026626;0.00257000678802604;0.00578886748225692;0.00257000678806637;1.97369506805033;0.00257000678806637]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984807e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.069631e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785395759505417;-9.97362810088841;-6.64792805207277]
Robot Arm 1 Joint Angles: [-0.00246154399070493;1.86379060493077;-0.00246154399069922;-0.0072948926543339;-0.0024615439907177;-1.7779186703491;-0.002461543277707]
Mobile Platform 2 Pose: [-2.35536560284969;9.97381049369144;6.37725595857499]
Robot Arm 2 Joint Angles: [0.00257000678950667;-1.92055422191637;0.0025700067894734;0.00578886747801027;0.00257000678951373;1.81149372970894;0.00257000678951373]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059253e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.186751e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785395759505416;-9.97178688319742;-6.65748565585436]
Robot Arm 1 Joint Angles: [-0.0024615439913949;2.15838543548645;-0.00246154399138918;-0.00729489265265837;-0.00246154399140765;-2.07251350090813;-0.00246154327839695]
Mobile Platform 2 Pose: [-2.35536560284969;9.97208283720559;6.37460130410423]
Robot Arm 2 Joint Angles: [0.00257000679041306;-2.19697925965002;0.00257000679037979;0.00578886747531167;0.00257000679042012;2.08791876744798;0.00257000679042012]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857436e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.111267e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785395759505416;-9.97319545975326;-6.65010797578431]
Robot Arm 1 Joint Angles: [-0.00246154399180599;1.93301318655206;-0.00246154399180028;-0.00729489265168518;-0.00246154399181875;-1.84714125197569;-0.00246154327880805]
Mobile Platform 2 Pose: [-2.35536560284969;9.97401740506425;6.37769083096594]
Robot Arm 2 Joint Angles: [0.00257000679095006;-1.88744840226334;0.00257000679091679;0.00578886747375091;0.00257000679095712;1.77838791006442;0.00257000679095712]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905819e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.050753e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785395759505415;-9.97269674450398;-6.65290069318509]
Robot Arm 1 Joint Angles: [-0.00246154399205884;2.01280762643513;-0.00246154399205312;-0.00729489265107502;-0.0024615439920716;-1.92693569185998;-0.0024615432790609]
Mobile Platform 2 Pose: [-2.35536560284969;9.97232492091376;6.3748514802843]
Robot Arm 2 Joint Angles: [0.00257000679128658;-2.15824586634062;0.00257000679125332;0.00578886747274581;0.00257000679129364;2.04918537414371;0.00257000679129364]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634522e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.879784e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785395759505415;-9.97221078172521;-6.65519569974022]
Robot Arm 1 Joint Angles: [-0.00246154399221388;2.09056167103909;-0.00246154399220816;-0.00729489265070351;-0.00246154399222663;-2.00468973646468;-0.00246154327921593]
Mobile Platform 2 Pose: [-2.35536560284969;9.97346780201827;6.37684080149871]
Robot Arm 2 Joint Angles: [0.00257000679148826;-1.9753848896187;0.002570006791455;0.00578886747215782;0.00257000679149532;1.86632439742297;0.00257000679149532]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.067005e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.100887e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735006134901;-6.64870124197991]
Robot Arm 1 Joint Angles: [-0.00246154399230644;1.8841885886564;-0.00246154399230073;-0.0072948926504832;-0.00246154399231921;-1.79831665408243;-0.00246154327930851]
Mobile Platform 2 Pose: [-2.35536560284969;9.97325891467374;6.37626939797217]
Robot Arm 2 Joint Angles: [0.0025700067916118;-2.00880686474274;0.00257000679157854;0.00578886747179187;0.00257000679161886;1.89974637254775;0.00257000679161886]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.069100e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.392690e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163094321734;-6.65826420090662]
Robot Arm 1 Joint Angles: [-0.00246154399236456;2.18333583229786;-0.00246154399235883;-0.00729489265034229;-0.00246154399237731;-2.09746389772418;-0.0024615432793666]
Mobile Platform 2 Pose: [-2.35536560284969;9.97250775506483;6.37527741705332]
Robot Arm 2 Joint Angles: [0.00257000679168798;-2.12899240216762;0.00257000679165472;0.00578886747156687;0.00257000679169505;2.01993190997307;0.00257000679169505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.776572e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.529015e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785395759505415;-9.97354092061521;-6.64840078543374]
Robot Arm 1 Joint Angles: [-0.00246154399239889;1.87773944863744;-0.00246154399239318;-0.00729489265026122;-0.00246154399241166;-1.79186751406391;-0.00246154327940096]
Mobile Platform 2 Pose: [-2.35536560284969;9.97395595602952;6.37746424747336]
Robot Arm 2 Joint Angles: [0.00257000679173342;-1.89728024781795;0.00257000679170015;0.00578886747143412;0.00257000679174048;1.78821975562367;0.00257000679174048]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247228e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.422081e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.785395759505415;-9.97212038216775;-6.6558824117393]
Robot Arm 1 Joint Angles: [-0.00246154399242036;2.10502560023202;-0.00246154399241463;-0.00729489265020912;-0.00246154399243311;-2.0191536656586;-0.00246154327942241]
Mobile Platform 2 Pose: [-2.35536560284969;9.97204441460835;6.37444314131024]
Robot Arm 2 Joint Angles: [0.00257000679176198;-2.203126875204;0.00257000679172872;0.00578886747134883;0.00257000679176905;2.09406638300989;0.00257000679176905]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064539e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.883807e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785395759505415;-9.97279446002223;-6.65223019561338]
Robot Arm 1 Joint Angles: [-0.00246154399243326;1.99717314351447;-0.00246154399242755;-0.00729489265017845;-0.00246154399244602;-1.91130120894111;-0.00246154327943532]
Mobile Platform 2 Pose: [-2.35536560284969;9.9739248537481;6.37750159496321]
Robot Arm 2 Joint Angles: [0.0025700067917789;-1.90225661284408;0.00257000679174563;0.00578886747129974;0.00257000679178596;1.79319612065007;0.00257000679178596]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.742991e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.334314e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.785395759505415;-9.97311524782296;-6.65079308472694]
Robot Arm 1 Joint Angles: [-0.00246154399244109;1.94584709539847;-0.00246154399243538;-0.00729489265015967;-0.00246154399245386;-1.85997516082515;-0.00246154327944316]
Mobile Platform 2 Pose: [-2.35536560284969;9.97257939634751;6.37516057547322]
Robot Arm 2 Joint Angles: [0.00257000679178947;-2.1175297969394;0.0025700067917562;0.00578886747126819;0.00257000679179653;2.00846930474545;0.00257000679179653]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155980e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.246157e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785395759505415;-9.97184705328127;-6.6571492964578]
Robot Arm 1 Joint Angles: [-0.00246154399244598;2.14875822206857;-0.00246154399244025;-0.00729489265014786;-0.00246154399245873;-2.06288628749528;-0.00246154327944803]
Mobile Platform 2 Pose: [-2.35536560284969;9.97318117664317;6.37631926714634]
Robot Arm 2 Joint Angles: [0.00257000679179584;-2.02124494963267;0.00257000679176257;0.00578886747124957;0.0025700067918029;1.91218445743876;0.0025700067918029]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302894e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.145755e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362257766344;-6.64808091524349]
Robot Arm 1 Joint Angles: [-0.00246154399244884;1.86467432092128;-0.00246154399244313;-0.00729489265014114;-0.00246154399246161;-1.778802386348;-0.0024615432794509]
Mobile Platform 2 Pose: [-2.35536560284969;9.97353588035173;6.37666475172753]
Robot Arm 2 Joint Angles: [0.00257000679179971;-1.96449235626335;0.00257000679176644;0.00578886747123817;0.00257000679180677;1.85543186406946;0.00257000679180677]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531498e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.805189e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.785395759505415;-9.97171662409789;-6.6579385588234]
Robot Arm 1 Joint Angles: [-0.00246154399245068;2.16962689140944;-0.00246154399244495;-0.00729489265013662;-0.00246154399246343;-2.08375495683617;-0.00246154327945273]
Mobile Platform 2 Pose: [-2.35536560284969;9.97226935151155;6.37481719920749]
Robot Arm 2 Joint Angles: [0.00257000679180212;-2.16713697069151;0.00257000679176886;0.00578886747123097;0.00257000679180919;2.05807647849764;0.00257000679180919]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.613167e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.017110e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785395759505415;-9.97330001676718;-6.64968226679352]
Robot Arm 1 Joint Angles: [-0.00246154399245173;1.91628406432255;-0.00246154399244602;-0.00729489265013417;-0.0024615439924645;-1.83041212974928;-0.0024615432794538]
Mobile Platform 2 Pose: [-2.35536560284969;9.97402949441038;6.37755715655057]
Robot Arm 2 Joint Angles: [0.00257000679180354;-1.88551410687936;0.00257000679177027;0.0057888674712269;0.0025700067918106;1.77645361468549;0.0025700067918106]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408568e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.602206e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785395759505415;-9.97255494590405;-6.6537371298561]
Robot Arm 1 Joint Angles: [-0.00246154399245241;2.03549540242296;-0.00246154399244669;-0.00729489265013252;-0.00246154399246517;-1.9496234678497;-0.00246154327945446]
Mobile Platform 2 Pose: [-2.35536560284969;9.97210958989574;6.37444483712915]
Robot Arm 2 Joint Angles: [0.00257000679180445;-2.19269882922238;0.00257000679177119;0.00578886747122412;0.00257000679181151;2.08363833702852;0.00257000679181151]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394164e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.407437e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785395759505415;-9.97234650754405;-6.65460270477929]
Robot Arm 1 Joint Angles: [-0.00246154399245282;2.06884554002401;-0.0024615439924471;-0.00729489265013153;-0.00246154399246557;-1.98297360545075;-0.00246154327945487]
Mobile Platform 2 Pose: [-2.35536560284969;9.97376137628911;6.37719022292353]
Robot Arm 2 Joint Angles: [0.00257000679180497;-1.92841300628333;0.00257000679177171;0.00578886747122265;0.00257000679181203;1.81935251408947;0.00257000679181203]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956357e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.583572e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785395759505415;-9.97343026926908;-6.64918949930142]
Robot Arm 1 Joint Angles: [-0.00246154399245304;1.89544366401808;-0.00246154399244733;-0.00729489265013102;-0.00246154399246581;-1.80957172944482;-0.00246154327945511]
Mobile Platform 2 Pose: [-2.35536560284969;9.97287638384075;6.37554958303447]
Robot Arm 2 Joint Angles: [0.00257000679180531;-2.07001179802025;0.00257000679177205;0.00578886747122162;0.00257000679181237;1.96095130582639;0.00257000679181237]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260469e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.440795e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164968951921;-6.65826235962286]
Robot Arm 1 Joint Angles: [-0.00246154399245322;2.18033642399812;-0.0024615439924475;-0.00729489265013054;-0.00246154399246597;-2.09446448942486;-0.00246154327945527]
Mobile Platform 2 Pose: [-2.35536560284969;9.97287409296167;6.37576117175334]
Robot Arm 2 Joint Angles: [0.00257000679180551;-2.07037833867356;0.00257000679177225;0.00578886747122103;0.00257000679181257;1.9613178464797;0.00257000679181257]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807923e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.944489e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785395759505415;-9.97358545598195;-6.64829065134618]
Robot Arm 1 Joint Angles: [-0.00246154399245328;1.87061378995892;-0.00246154399244758;-0.00729489265013045;-0.00246154399246605;-1.78474185538566;-0.00246154327945535]
Mobile Platform 2 Pose: [-2.35536560284969;9.97376285596812;6.37699431004016]
Robot Arm 2 Joint Angles: [0.00257000679180563;-1.92817625764118;0.00257000679177236;0.00578886747122071;0.00257000679181269;1.81911576544733;0.00257000679181269]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620323e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.836822e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.785395759505415;-9.97200013539397;-6.65659579684792]
Robot Arm 1 Joint Angles: [-0.00246154399245336;2.12426508403601;-0.00246154399244764;-0.00729489265013021;-0.00246154399246612;-2.03839314946275;-0.00246154327945542]
Mobile Platform 2 Pose: [-2.35536560284969;9.97210872702559;6.37447359698393]
Robot Arm 2 Joint Angles: [0.00257000679180571;-2.19283688844622;0.00257000679177245;0.00578886747122041;0.00257000679181278;2.08377639625236;0.00257000679181278]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640783e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.328767e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785395759505415;-9.97292968713781;-6.65164343752433]
Robot Arm 1 Joint Angles: [-0.00246154399245338;1.9755368050224;-0.00246154399244767;-0.00729489265013019;-0.00246154399246615;-1.88966487044914;-0.00246154327945544]
Mobile Platform 2 Pose: [-2.35536560284969;9.97402921926186;6.37752634882069]
Robot Arm 2 Joint Angles: [0.00257000679180575;-1.88555813064341;0.00257000679177248;0.00578886747122037;0.00257000679181281;1.77649763844955;0.00257000679181281]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711110e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.504460e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785395759505415;-9.97299183541534;-6.65154595274172]
Robot Arm 1 Joint Angles: [-0.0024615439924534;1.96559308061636;-0.00246154399244769;-0.00729489265013015;-0.00246154399246617;-1.8797211460431;-0.00246154327945546]
Mobile Platform 2 Pose: [-2.35536560284969;9.97227088670467;6.37459863982407]
Robot Arm 2 Joint Angles: [0.00257000679180579;-2.1668913397927;0.00257000679177252;0.0057888674712202;0.00257000679181285;2.05783084759884;0.00257000679181285]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373557e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.301025e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785395759505415;-9.97194709741018;-6.65673575840169]
Robot Arm 1 Joint Angles: [-0.00246154399245343;2.13275116144264;-0.00246154399244771;-0.00729489265013005;-0.00246154399246618;-2.04687922686938;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.35536560284969;9.97353397864286;6.37676956915511]
Robot Arm 2 Joint Angles: [0.00257000679180579;-1.9647966296833;0.00257000679177253;0.00578886747122021;0.00257000679181285;1.85573613748944;0.00257000679181285]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767804e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.026391e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360123983821;-6.64831637142007]
Robot Arm 1 Joint Angles: [-0.00246154399245341;1.86808837295744;-0.00246154399244771;-0.00729489265013014;-0.00246154399246618;-1.78221643838418;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.35536560284969;9.97318343489819;6.37597108332966]
Robot Arm 2 Joint Angles: [0.00257000679180581;-2.0208836288305;0.00257000679177254;0.00578886747122016;0.00257000679181287;1.91182313663664;0.00257000679181287]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002526e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.586941e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166632921239;-6.65828886791101]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17767407308892;-0.00246154399244772;-0.00729489265013;-0.0024615439924662;-2.09180213851566;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35536560284969;9.97257730544528;6.37521519333999]
Robot Arm 2 Joint Angles: [0.00257000679180582;-2.11786434129547;0.00257000679177255;0.00578886747122012;0.00257000679181288;2.00880384910162;0.00257000679181288]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.187228e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.755215e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785395759505415;-9.97339148554344;-6.64932526492237]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90164906012155;-0.00246154399244771;-0.00729489265013011;-0.00246154399246619;-1.81577712554829;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35536560284969;9.97392583478575;6.37722965735124]
Robot Arm 2 Joint Angles: [0.00257000679180581;-1.9020996468204;0.00257000679177255;0.00578886747122017;0.00257000679181287;1.79303915462654;0.00257000679181287]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546799e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.484199e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785395759505415;-9.97241328591609;-6.65456938368534]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.05816100049619;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.97228906592293;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35536560284969;9.97204432731813;6.37427577952989]
Robot Arm 2 Joint Angles: [0.00257000679180583;-2.20314084164002;0.00257000679177257;0.00578886747122007;0.00257000679181289;2.09408034944617;0.00257000679181289]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.136121e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.477551e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724870348897;-6.65398600376929]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.04636116471989;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.96048923014663;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35536560284969;9.97395513635271;6.37736895948152]
Robot Arm 2 Joint Angles: [0.00257000679180582;-1.89741139610626;0.00257000679177255;0.00578886747122016;0.00257000679181288;1.7883509039124;0.00257000679181288]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235454e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.793489e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785395759505415;-9.97334546906772;-6.64975107323676]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90901169623616;-0.00246154399244772;-0.00729489265013009;-0.00246154399246619;-1.8231397616629;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35536560284969;9.97250978769965;6.37488075481345]
Robot Arm 2 Joint Angles: [0.00257000679180583;-2.12866718059573;0.00257000679177256;0.00578886747122009;0.00257000679181289;2.01960668840188;0.00257000679181289]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.018100e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.472616e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785395759505415;-9.97169001677896;-6.65815098765241]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17388406243769;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.08801212786443;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35536560284969;9.97325672154781;6.37626407397797]
Robot Arm 2 Joint Angles: [0.00257000679180582;-2.00915776488985;0.00257000679177256;0.00578886747122012;0.00257000679181288;1.90009727269599;0.00257000679181288]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924401e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.036846e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.781930784856786;-9.97361420834274;-6.64826261384512]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98844282955444;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.7934878166058;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.35536560284969;9.97346982606615;6.37637726885016]
Robot Arm 2 Joint Angles: [0.00257000679180582;-1.97506104195553;0.00257000679177256;0.00578886747122013;0.00257000679181288;1.86600054976168;0.00257000679181288]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.273530e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.020149e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.781930784856786;-9.97188691446132;-6.65726742696511]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.26480985058202;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.06985483763338;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.35536560284969;9.9723232619642;6.37473171874389]
Robot Arm 2 Joint Angles: [0.00257000679180583;-2.15851129826751;0.00257000679177257;0.00578886747122007;0.00257000679181289;2.04945080607365;0.00257000679181289]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.621431e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.884592e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.781930784856786;-9.9732323443963;-6.65020846683966]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.04954106098514;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.8545860480365;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.35536560284969;9.97401784934326;6.37735278952131]
Robot Arm 2 Joint Angles: [0.00257000679180582;-1.88737731761845;0.00257000679177255;0.00578886747122016;0.00257000679181288;1.7783168254246;0.00257000679181288]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.606071e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.789635e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.781930784856786;-9.97261056418662;-6.65362865641497]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.14902589453354;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.9540708815849;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.35355773104264;9.97208354218883;6.37423726633542]
Robot Arm 2 Joint Angles: [0.00257000492916068;-2.07281855111014;0.00257000492912742;0.00578886933386521;0.00257000492916775;2.07284113675501;0.00257000492916775]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.476853e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.001647e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.781930784856786;-9.97248553565259;-6.65406498247295]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.0943951023932;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.97407544516737;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35355773104264;9.97380250734812;6.37707989000801]
Robot Arm 2 Joint Angles: [0.00257000492916067;-1.79778412562409;0.0025700049291274;0.0057888693338653;0.00257000492916773;1.79780671126896;0.00257000492916773]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.174689e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.283084e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.781930784856786;-9.97332103578314;-6.64994660308826]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.96071508150572;-0.00246154399244772;-0.00729489265013009;-0.0024615439924662;-1.84039542427989;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35355773104264;9.97300383740322;6.3755671167564]
Robot Arm 2 Joint Angles: [0.00257000492916067;-1.92557131680672;0.00257000492912741;0.00578886933386526;0.00257000492916774;1.92559390245159;0.00257000492916774]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.334760e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.101996e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.781930784856786;-9.97172467835556;-6.65803336534504]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.21613226991864;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.09581261269281;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35355773104264;9.97266185252561;6.37523912431206]
Robot Arm 2 Joint Angles: [0.00257000492916068;-1.98028889722473;0.00257000492912741;0.00578886933386524;0.00257000492916774;1.98031148286959;0.00257000492916774]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.170335e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.979789e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.781930784856786;-9.97362000881263;-6.64831093503223]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.03069103730291;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80128830144905;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.35175327514686;9.97398597582524;6.3772072129445]
Robot Arm 2 Joint Angles: [0.00257000679180582;-1.79484766420033;0.00257000679177255;0.00578886747122016;0.00257000679181288;1.76845175679206;0.00257000679181288]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.391694e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.145261e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.781930784856786;-9.97179192618148;-6.65780398234219]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.09439510486436;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.09378152025455;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35175327514686;9.97206648716968;6.37416715322599]
Robot Arm 2 Joint Angles: [0.00257000679180583;-2.10196584908927;0.00257000679177257;0.00578886747122006;0.0025700067918129;2.07556994168101;0.0025700067918129]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.334228e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.439234e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.781930784856786;-9.97336422103943;-6.64960458626882]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.84282792759109;-0.00246154399244772;-0.00729489265013009;-0.0024615439924662;-1.84221434298129;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34995424287834;9.97387796739539;6.3771377374197]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.91652461642401;0.0025700086544177;0.005788865608575;0.00257000865445803;1.78573310772724;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.623085e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.965914e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.781930784856786;-9.97278141309306;-6.65282848474526]
Robot Arm 1 Joint Angles: [-0.00246154399245344;1.93607719901045;-0.00246154399244772;-0.00729489265013005;-0.0024615439924662;-1.93546361440065;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34995424287834;9.97284542315972;6.37526404366503]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.08173169413144;0.00257000865441771;0.00578886560857495;0.00257000865445804;1.95094018543468;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.167411e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.596718e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.780787317887678;-9.97222874028549;-6.65547088638468]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.02450484635908;-0.00246154212980258;-0.00729489451277517;-0.00246154212982105;-2.02389126174927;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97304508626235;6.37580046749758]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.04978559771082;0.00257000865441771;0.00578886560857496;0.00257000865445804;1.91899408901405;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.649071e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.090618e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.780787317887678;-9.97367117069468;-6.64817247881907]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.79371598088929;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.79310239627949;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97374853010476;6.376718011799]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.93723458292535;0.0025700086544177;0.00578886560857499;0.00257000865445803;1.80644307422858;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.180042e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.601804e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.780787317887678;-9.97173870277627;-6.65810674158607]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.10291084783463;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.10229726322483;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97222758520509;6.37443745758429]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.18058576687174;0.00257000865441772;0.00578886560857492;0.00257000865445804;2.04979425817498;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.507501e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.741126e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.780787317887678;-9.97348434472244;-6.64904240375458]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.823608136447;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.8229945518372;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97410078927563;6.37738950496854]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.88087311558513;0.0025700086544177;0.00578886560857501;0.00257000865445803;1.75008160688836;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.854000e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.163466e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.780787317887678;-9.97260431804172;-6.65378612306461]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.96441240536281;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.96379882075301;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97224961052961;6.37434344292065]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.17706171494888;0.00257000865441772;0.00578886560857492;0.00257000865445804;2.04627020625212;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.364562e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.064684e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.780787317887678;-9.97239326444753;-6.65466834748365]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.99818098043266;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.99756739582286;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97371760221841;6.37682398221779]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.9421830447411;0.0025700086544177;0.00578886560857499;0.00257000865445803;1.81139153604433;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.161111e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.756112e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.780787317887678;-9.97360077486122;-6.64860602062932]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.80497931424309;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.80436572963329;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97308777223087;6.37557630676859]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.04295584274722;0.00257000865441771;0.00578886560857496;0.00257000865445804;1.91216433405046;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.325954e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.116497e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.780787317887678;-9.97171836170423;-6.658241253747]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.10616541936123;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.10555183475143;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.972803766795;6.37534551212825]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.08839671248687;0.00257000865441771;0.00578886560857495;0.00257000865445804;1.9576052037901;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.022071e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.689699e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.780787317887678;-9.97358292114693;-6.64859350406936]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.80783590852934;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.80722232391954;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.9739019347851;6.37692745521438]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.91268983407076;0.0025700086544177;0.005788865608575;0.00257000865445803;1.781898325374;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.353196e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.961309e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.780787317887678;-9.97242929582516;-6.65472818351729]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.9924159600121;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.9918023754023;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97213651903709;6.37420526511324]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.19515635375254;0.00257000865441772;0.00578886560857491;0.00257000865445804;2.06436484505577;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.850119e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.024251e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.780787317887678;-9.97256710722439;-6.65381960609768]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.97036613613508;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.96975255152528;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.9740738414276;6.37730563856999]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.88518477127119;0.0025700086544177;0.00578886560857501;0.00257000865445803;1.75439326257443;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.662316e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.848819e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.780787317887678;-9.97350697866232;-6.64915901017857]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.81998670606583;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.81937312145603;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97240581483301;6.37449647161664]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.15206902640551;0.00257000865441772;0.00578886560857493;0.00257000865445804;2.02127751770874;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.671673e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.102863e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.780787317887678;-9.97173165626182;-6.65820425042283]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.10403829014696;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.10342470553715;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97352103376706;6.37644852407008]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.97363399695753;0.00257000865441771;0.00578886560857498;0.00257000865445803;1.84284248826077;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.766835e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.394642e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.780787317887678;-9.9736583561324;-6.64826629568792]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.79576631085434;-0.00246154212980257;-0.00729489451277526;-0.00246154212982105;-1.79515272624454;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97332674449034;6.37589446381746]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.00472028123216;0.00257000865441771;0.00578886560857497;0.00257000865445803;1.8739287725354;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.329298e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.723206e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.780787317887678;-9.97226234157743;-6.65562438396957]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.01912863964951;-0.00246154212980258;-0.00729489451277517;-0.00246154212982105;-2.01851505503971;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97257781489977;6.37491200452188]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.12454901572311;0.00257000865441771;0.00578886560857494;0.00257000865445804;1.99375750702634;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.031706e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.864320e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.780787317887678;-9.97274426905793;-6.65295537646724]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.94202024276835;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.94140665815855;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97401320792133;6.37707285115887]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.89488613227352;0.0025700086544177;0.00578886560857501;0.00257000865445803;1.76409462357676;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.870797e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.720787e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.780787317887678;-9.97339125011169;-6.64982275905802]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.83850327416806;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.83788968955826;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97210825487821;6.37406845584824]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.19967861917204;0.00257000865441772;0.00578886560857491;0.00257000865445804;2.06888711047528;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.281232e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.051423e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.780787317887678;-9.97177809210851;-6.65799869980205]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.09660855467668;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.09599497006688;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97400135408468;6.37714317337403]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.896782746137;0.0025700086544177;0.00578886560857501;0.00257000865445803;1.76599123744024;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.263862e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.892699e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.780787317887678;-9.97370961346422;-6.6480661900524]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.78756513776316;-0.00246154212980257;-0.00729489451277526;-0.00246154212982105;-1.78695155315336;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97260545767813;6.37472298669216]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.12012617118561;0.00257000865441771;0.00578886560857494;0.00257000865445804;1.98933466248885;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.116333e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.167634e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.780787317887678;-9.9721093371347;-6.65644525184617]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.04360935048587;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.04299576587607;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97329672575463;6.37602581021673]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.00952327894526;0.00257000865441771;0.00578886560857497;0.00257000865445803;1.8787317702485;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.172226e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.036540e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.780787317887678;-9.97291889672291;-6.65210569829201]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.91407981637125;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.91346623176145;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97354809142609;6.37619568765303]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.96930477151221;0.00257000865441771;0.00578886560857498;0.00257000865445803;1.83851326281544;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.821757e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.636264e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.780787317887678;-9.97325572432746;-6.65058519754467]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.86018739964406;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.85957381503426;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97238301719474;6.37452430606482]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.15571664852828;0.00257000865441772;0.00578886560857493;0.00257000865445804;2.02492513983152;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.509281e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.643848e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.780787317887678;-9.97185595432522;-6.65763366927107]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.08415060000165;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08353701539185;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97407988591591;6.37714682307629]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.88421765314106;0.0025700086544177;0.00578886560857501;0.00257000865445803;1.7534261444443;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.841102e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.944596e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.780787317887678;-9.97373608387173;-6.64799622448295]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.78332987256037;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78271628795057;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97214482321368;6.37403136084925]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.19382768549884;0.00257000865441772;0.00578886560857491;0.00257000865445804;2.06303617680207;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.376145e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.038085e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.780787317887678;-9.97197575128362;-6.65716358467707]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.06498308665821;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.06436950204841;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97388449064507;6.37690408325243]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.91548089647515;0.0025700086544177;0.005788865608575;0.00257000865445803;1.78468938777839;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.288538e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.269338e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.780787317887678;-9.97308559266924;-6.65129837034025]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.88740846495945;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.88679488034965;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97283430180545;6.37500337995138]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.08351111081557;0.00257000865441771;0.00578886560857495;0.00257000865445804;1.9527196021188;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.357616e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.734911e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.780787317887678;-9.97310332380057;-6.65143036183018]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.88457148394665;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.88395789933685;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97305647622314;6.37557519025139]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.04796320398374;0.00257000865441771;0.00578886560857496;0.00257000865445804;1.91717169528698;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.062425e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.698106e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.780787317887678;-9.97196239137556;-6.65712391246909]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.06712067194765;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.06650708733785;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97374042348663;6.37646021174285]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.93853164182561;0.0025700086544177;0.00578886560857499;0.00257000865445803;1.80774013312885;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.018862e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.768749e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.780787317887678;-9.97373748399289;-6.64805754600095]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.78310585317513;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78249226856533;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34995424287834;9.97223329541396;6.37420384891543]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.17967213345251;0.00257000865441772;0.00578886560857492;0.00257000865445804;2.04888062475575;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.932415e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.375935e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.780787317887678;-9.97186639578853;-6.65775562113772]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.08247996587237;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08186638126257;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97410094398302;6.37714487335895]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.88084836240409;0.0025700086544177;0.00578886560857501;0.00257000865445803;1.75005685370732;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.846636e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.545377e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.780787317887678;-9.97323965492944;-6.65055772081822]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.86275850332766;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.86214491871786;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97224359930868;6.374091739987]
Robot Arm 2 Joint Angles: [0.00257000865445098;-2.17802351029794;0.00257000865441772;0.00578886560857492;0.00257000865445804;2.04723200160118;0.00257000865445804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.869848e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.940473e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.780787317887678;-9.9729378265876;-6.65233813447082]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.91105103802114;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.91043745341134;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97372596033059;6.37659346562898]
Robot Arm 2 Joint Angles: [0.00257000865445097;-1.94084574679223;0.0025700086544177;0.00578886560857499;0.00257000865445803;1.81005423809547;0.00257000865445803]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.981933e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.118424e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.780787317887678;-9.97209353763987;-6.65648926978395]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.04613726965781;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.04552368504801;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34995424287834;9.97307643609821;6.37531479631998]
Robot Arm 2 Joint Angles: [0.00257000865445097;-2.04476962397353;0.00257000865441771;0.00578886560857496;0.00257000865445804;1.91397811527677;0.00257000865445804]
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 75)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',75,0)">第 75 行</a>)
    real_time_2D_plot(human_poses{k}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

{<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/hand_generate_human_trajectories.m',45,66)">文件: hand_generate_human_trajectories.m 行: 45 列: 66</a>
无效表达式。调用函数或对变量进行索引时，请使用圆括号。否则，请检查不匹配的分隔符。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 25)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',25,0)">第 25 行</a>)
planar_poses = hand_generate_human_trajectories(simulation_time);
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',25,0)
hand_generate_human_trajectories(simulation_time)
{<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/hand_generate_human_trajectories.m',45,66)">文件: hand_generate_human_trajectories.m 行: 45 列: 66</a>
无效表达式。调用函数或对变量进行索引时，请使用圆括号。否则，请检查不匹配的分隔符。
} 
planar_poses = hand_generate_human_trajectories(simulation_time)
{<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/hand_generate_human_trajectories.m',45,66)">文件: hand_generate_human_trajectories.m 行: 45 列: 66</a>
无效表达式。调用函数或对变量进行索引时，请使用圆括号。否则，请检查不匹配的分隔符。
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.547848e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 Joint Angles: [-0.000121114227683022;0.185441233024398;-0.000121114227681381;1.86264489701995e-09;-0.000121114227682768;-0.22518584865809;-0.00012111422768375]
Mobile Platform 2 Pose: [-2.35619437191491;9.99866771753545;9.81844390937036]
Robot Arm 2 Joint Angles: [0.000121114227682424;-0.185441233024398;0.000121114227682774;-1.86264487453752e-09;0.000121114227682774;0.213165194053599;0.000121114227682774]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.026148e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 Joint Angles: [-0.000242257227143282;0.370882466048795;-0.00024225722714405;-0.000118135763669368;-0.000242257227146825;-0.450777934956321;-0.000242257227154676]
Mobile Platform 2 Pose: [-2.35619425360615;9.99733167719312;9.63690106041641]
Robot Arm 2 Joint Angles: [0.000242260535616737;-0.370882466048795;0.000242260535617437;8.24406669025269e-05;0.000242260535617437;0.426849204164602;0.000242260535617437]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.694859e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 Joint Angles: [-0.000363286279799942;0.556323699073193;-0.000363286279798916;-0.000357282809229051;-0.000363286279802673;-0.676844213293337;-0.000363286279809542]
Mobile Platform 2 Pose: [-2.35619413529215;9.99599115574577;9.45545454589275]
Robot Arm 2 Joint Angles: [0.000363412210349703;-0.556323699073193;0.000363412210350752;0.000250017184069795;0.000363412210350752;0.641165057095127;0.000363412210350752]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.519414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 Joint Angles: [-0.000483866411480727;0.74176493209759;-0.000483866411478066;-0.000719510076232192;-0.000483866411482804;-0.903917115513842;-0.000483866411488692]
Mobile Platform 2 Pose: [-2.35619401716568;9.99464273095435;9.27451030558023]
Robot Arm 2 Joint Angles: [0.000484371855473429;-0.74176493209759;0.000484371855474827;0.000505156626171682;0.000484371855474827;0.856657882162591;0.000484371855474827]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.474880e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 Joint Angles: [-0.0006036745926073;0.927206165121988;-0.000603674592606945;-0.00120731922081321;-0.000603674592618552;-1.13251733087994;-0.000603674592617571]
Mobile Platform 2 Pose: [-2.35619389940294;9.99328288341341;9.09451230889671]
Robot Arm 2 Joint Angles: [0.000604959038630923;-0.927206165121988;0.000604959038632667;0.000851202889335113;0.000604959038632667;1.07388744034533;0.000604959038632667]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.543782e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346623]
Robot Arm 1 Joint Angles: [-0.000722796083224335;1.11264739814639;-0.00072279608322361;-0.00182553790636024;-0.000722796083242086;-1.36238137284088;-0.000722796083234236]
Mobile Platform 2 Pose: [-2.35619378181687;9.99191235316415;8.91540999400951]
Robot Arm 2 Joint Angles: [0.000725365317144262;-1.11264739814639;0.000725365317146352;0.00129364656158718;0.000725365317146352;1.29272983446354;0.000725365317146352]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528709;-9.99002831581144;-8.79949883355178]
Robot Arm 1 Joint Angles: [-0.000841580556365119;1.29808863117078;-0.000841580556358477;-0.00257974514714843;-0.000841580556377935;-1.59288971083287;-0.000841580556369103]
Mobile Platform 2 Pose: [-2.35619366397363;9.99053427603202;8.73682165238347]
Robot Arm 2 Joint Angles: [0.000846034930370441;-1.29808863117078;0.000846034930372875;0.00183861009437648;0.000846034930372875;1.51267720998908;0.000846034930372875]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274173]
Robot Arm 1 Joint Angles: [-0.000959829987060112;1.48352986419518;-0.000959829987047145;-0.00347025803777765;-0.000959829987065621;-1.82450740028498;-0.00095982998705679]
Mobile Platform 2 Pose: [-2.35619354585768;9.98914653918635;8.55900645298953]
Robot Arm 2 Joint Angles: [0.000966983802703701;-1.48352986419518;0.000966983802706478;0.00248885214782629;0.000966983802706478;1.7340648611722;0.000966983802706478]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196129;-9.98711511781;-8.46062525935787]
Robot Arm 1 Joint Angles: [-0.00107743739451576;1.66897109721958;-0.0010774373945052;-0.00449854422257743;-0.00107743739452368;-2.05708258800973;-0.00107743739451485]
Mobile Platform 2 Pose: [-2.35619342737779;9.9877492297113;8.38210654928569]
Robot Arm 2 Joint Angles: [0.00108830534787571;-1.66897109721958;0.00108830534787883;0.0032480225001942;0.00108830534787883;1.95687520476809;0.00108830534787883]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929674;-9.98565931998688;-8.29072857539468]
Robot Arm 1 Joint Angles: [-0.00119546838174874;1.85441233024398;-0.00119546838174059;-0.00567665166884611;-0.00119546838175906;-2.0943951023932;-0.00119546838175023]
Mobile Platform 2 Pose: [-2.35619330760959;9.98635047565697;8.20503460456464]
Robot Arm 2 Joint Angles: [0.0012109461293282;-1.85441233024398;0.00121094612932479;0.00412547116270929;0.00121094612933166;2.0943951023932;0.00121094612933166]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.9841823282427;-8.12462828169497]
Robot Arm 1 Joint Angles: [-0.00131696123650541;2.03985356326837;-0.00131696123649832;-0.00639922083539073;-0.00131696123651679;-2.30399143679527;-0.00131696052350609]
Mobile Platform 2 Pose: [-2.35619318738723;9.98493698763025;8.03016739579692]
Robot Arm 2 Joint Angles: [0.001334053022404;-2.03985356326837;0.00133405302240003;0.00485303802587016;0.0013340530224078;2.29681337673631;0.00133405189215781]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840344088;-7.9599482888563]
Robot Arm 1 Joint Angles: [-0.00143870942785493;2.14949927567626;-0.00143870942784766;-0.00719336673714352;-0.00143870942786614;-2.22826414941097;-0.00143870871485544]
Mobile Platform 2 Pose: [-2.35619306614638;9.98351838014246;7.85622888166466]
Robot Arm 2 Joint Angles: [0.00145820234315693;-2.14635972013224;0.00145820234314848;0.00563276907974568;0.00145820234316106;2.22297982475799;0.00145820121291107]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455234098;-7.79314279536571]
Robot Arm 1 Joint Angles: [-0.00156630119507468;2.18077941225243;-0.00156630119506725;-0.00742071448357543;-0.00156630119508573;-2.20427246494267;-0.00156630048207503]
Mobile Platform 2 Pose: [-2.35568417201985;9.98211894877797;7.68191990727737]
Robot Arm 2 Joint Angles: [0.00158496881301765;-2.17722407754034;0.00158496881300473;0.00585698712128845;0.00158496881302212;2.20007557732082;0.00158496768277212]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.9797235609508;-7.63794018922978]
Robot Arm 1 Joint Angles: [-0.00168692949029622;2.19333238380943;-0.00168692949028909;-0.00746871767647478;-0.00168692949030756;-2.20035149691845;-0.00168692877729686]
Mobile Platform 2 Pose: [-2.35531792827297;9.98066883537543;7.51575072500617]
Robot Arm 2 Joint Angles: [0.00170680905832085;-2.18886429599196;0.00170680905830344;0.00590855236576529;0.00170680905832564;2.19568880599711;0.00170680792807565]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591665116;-7.46636918684545]
Robot Arm 1 Joint Angles: [-0.00182021652145274;2.1916426546638;-0.00182021652144546;-0.00746685601467987;-0.00182021652146393;-2.1937495063759;-0.00182021580845323]
Mobile Platform 2 Pose: [-2.35478869897052;9.97930450515461;7.33821419300697]
Robot Arm 2 Joint Angles: [0.00183824853028427;-2.18822457810052;0.00183824853026239;0.00591144122124282;0.0018382485302894;2.19027019700647;0.00183824740003941]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346212;-9.97678818484225;-7.31817266015123]
Robot Arm 1 Joint Angles: [-0.00193743410109527;2.19689567720951;-0.00193743410108827;-0.0074507970691896;-0.00193743410110675;-2.19753736887274;-0.00193743338809605]
Mobile Platform 2 Pose: [-2.35397115067364;9.97782829207453;7.17879312086094]
Robot Arm 2 Joint Angles: [0.00195687793871714;-2.19339849454494;0.00195687793869076;0.00589984076875505;0.00195687793872258;2.19401914127053;0.00195687680847259]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232725;-9.9753590119579;-7.16418511934567]
Robot Arm 1 Joint Angles: [-0.00205940500907929;2.19373921813631;-0.00205940500907258;-0.00742961334509667;-0.00205940500909106;-2.19394443665204;-0.00205940429608036]
Mobile Platform 2 Pose: [-2.3527652485371;9.97645065930863;7.01004639490938]
Robot Arm 2 Joint Angles: [0.00208363086407466;-2.19021702585757;0.00208363086404379;0.00588292077498999;0.00208363086408042;2.19041337265382;0.00208362973383043]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.78539602676002;-9.97430990779222;-7.00084677780109]
Robot Arm 1 Joint Angles: [-0.00218788775603952;2.1745482189954;-0.00218788775603313;-0.00740588686305206;-0.00218788775605161;-2.17462402122166;-0.00218788704304091]
Mobile Platform 2 Pose: [-2.35259854144849;9.97523762174517;6.84194414915061]
Robot Arm 2 Joint Angles: [0.00221095365066028;-2.26975114892606;0.00221095365062704;0.00586456807022921;0.00221095365066631;2.26997049578745;0.00221095252041632]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.78539595660333;-9.97259808478388;-6.91581468798367]
Robot Arm 1 Joint Angles: [-0.00225972690260118;2.20060448127733;-0.00225972690259496;-0.00739236789516434;-0.00225972690261343;-2.20063535217658;-0.00225972618960273]
Mobile Platform 2 Pose: [-2.35244116710607;9.97363430675014;6.72929638406058]
Robot Arm 2 Joint Angles: [0.00229643791955559;-2.42046328922182;0.00229643791952269;0.00585234549193177;0.00229646619515228;2.43619368554549;0.00229643678931196]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227853;-9.97409553094106;-6.77330520679708]
Robot Arm 1 Joint Angles: [-0.00236353352892908;2.01516324825293;-0.00236353352892313;-0.00737275824162398;-0.0023635335289416;-1.96106357862124;-0.0023635328159309]
Mobile Platform 2 Pose: [-2.35180306827204;9.97448146977537;6.56761684078381]
Robot Arm 2 Joint Angles: [0.00242483169193472;-2.23502205619742;0.002424831691902;0.0058837687472532;0.0024248599675316;2.14166937427145;0.00242483056169128]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.97228129699828;-6.73833367577277]
Robot Arm 1 Joint Angles: [-0.0023973975264219;2.09439509667864;-0.00239739752641602;-0.00731935067018818;-0.00239739752643449;-2.09439509417083;-0.00239739681342379]
Mobile Platform 2 Pose: [-2.35180302922265;9.97259577203783;6.51356452639028]
Robot Arm 2 Joint Angles: [0.00246481696698018;-2.22706665459637;0.00246481696694757;0.00578371080806401;0.00246484524257717;2.19912089058857;0.00246481583673685]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540074;-9.97333331315079;-6.69737167009219]
Robot Arm 1 Joint Angles: [-0.00242465195277759;1.92606732656943;-0.00242465195277178;-0.00731416496329334;-0.00242465195279026;-1.9260776954754;-0.00242465123977956]
Mobile Platform 2 Pose: [-2.3518029942045;9.9742868781858;6.47075750301407]
Robot Arm 2 Joint Angles: [0.00250067368585484;-2.04162542185502;0.00250067368582231;0.00575313286476677;0.00250070196145191;1.92857448671848;0.00250067255561159]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395781195851;-9.97294909644746;-6.68023583759776]
Robot Arm 1 Joint Angles: [-0.00243934043716969;1.98753916361096;-0.00243934043716392;-0.00731132947224667;-0.0024393404371824;-1.98755520349902;-0.0024393397241717]
Mobile Platform 2 Pose: [-2.35180297772205;9.97308993651695;6.44690139202074]
Robot Arm 2 Joint Angles: [0.00251754985887532;-2.0943951023932;0.00251754985884284;0.00570174108772994;0.00251757813447244;2.09439510239319;0.00251754872863212]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395772638942;-9.97200303864331;-6.67347995257358]
Robot Arm 1 Joint Angles: [-0.00244810084900341;2.0943951023932;-0.00244810084899765;-0.00730965080542708;-0.00244810084901613;-2.0943951023932;-0.00244810013600543]
Mobile Platform 2 Pose: [-2.35180296731042;9.973328082064;6.43381241730042]
Robot Arm 2 Joint Angles: [0.00252821136921079;-2.05629024785962;0.00252821136917833;0.00570017408236661;0.00252823964480793;2.05629338187035;0.00252821023896761]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395767401621;-9.97358488642586;-6.65848980933812]
Robot Arm 1 Joint Angles: [-0.00245346200287458;1.90895386925231;-0.00245346200286885;-0.00730863212378707;-0.00245346200288733;-1.84130047961619;-0.00245346128987663]
Mobile Platform 2 Pose: [-2.3518029612483;9.9739295958112;6.42662820989832]
Robot Arm 2 Joint Angles: [0.00253441897120497;-1.96004713338406;0.00253441897117253;0.0056992591582727;0.00253444724680213;1.96005209724298;0.00253441784096181]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395764311851;-9.9716500757212;-6.66429501601706]
Robot Arm 1 Joint Angles: [-0.00245662406489679;2.0943950978115;-0.00245662406489105;-0.00730248004082667;-0.00245662406490952;-2.09439510583112;-0.00245662335189882]
Mobile Platform 2 Pose: [-2.3518029577319;9.97235440030669;6.41956459255171]
Robot Arm 2 Joint Angles: [0.00253801790282427;-2.09439510918229;0.00253801790279185;0.00569872581046163;0.00253804617842144;2.09439509641609;0.00253801677258112]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785395762440954;-9.97334587567019;-6.65299730614471]
Robot Arm 1 Joint Angles: [-0.00245853800054772;1.90895386479396;-0.002458538000542;-0.00730211760364607;-0.00245853800056048;-1.82306748031366;-0.00245853728754978]
Mobile Platform 2 Pose: [-2.35180295561392;9.97427877556692;6.41985732209371]
Robot Arm 2 Joint Angles: [0.00254018485767403;-1.90895387562803;0.0025401848576416;0.00569840892516159;0.00254021313327119;1.7998707850637;0.00254018372743087]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785395761295571;-9.9725430019079;-6.65580126947701]
Robot Arm 1 Joint Angles: [-0.00245971087207978;2.03741082995867;-0.00245971087207406;-0.0072992808023168;-0.00245971087209253;-1.95153011908103;-0.00245971015908183]
Mobile Platform 2 Pose: [-2.35180295431256;9.97262176052371;6.41533366421172]
Robot Arm 2 Joint Angles: [0.00254151558835393;-2.09439510667072;0.00254151558832151;0.00569443956700369;0.00254154386395111;2.06499716122328;0.00254151445811079]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395760593853;-9.97237811570277;-6.65550571971024]
Robot Arm 1 Joint Angles: [-0.0024604294316762;2.06379090445736;-0.00246042943167048;-0.00729756247915696;-0.00246042943168896;-1.97791363022604;-0.00246042871867826]
Mobile Platform 2 Pose: [-2.35180295353315;9.97370806127453;6.41621305686288]
Robot Arm 2 Joint Angles: [0.002542313697188;-1.92058626998304;0.00254231369715558;0.00569372301038997;0.00254234197278518;1.89118975764883;0.00254231256694486]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395760172427;-9.97341831965915;-6.64975907402211]
Robot Arm 1 Joint Angles: [-0.00246086097205636;1.89735724181491;-0.00246086097205065;-0.00729653285710986;-0.00246086097206912;-1.81148202682769;-0.00246086025905842]
Mobile Platform 2 Pose: [-2.35180295306132;9.97368537077085;6.41532008294633]
Robot Arm 2 Joint Angles: [0.00254279685140224;-1.92421631003973;0.00254279685136982;0.00569328247895613;0.00254282512699942;1.89482067876839;0.0025427957211591]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395759909198;-9.97165700587365;-6.65837171221346]
Robot Arm 1 Joint Angles: [-0.00246113051844891;2.17916679493719;-0.00246113051844318;-0.00729588029970588;-0.00246113051846165;-2.09329288506478;-0.00246112980545095]
Mobile Platform 2 Pose: [-2.35180295277088;9.97262170074075;6.41341866898763]
Robot Arm 2 Joint Angles: [0.00254309426284033;-2.09440324378046;0.00254309426280791;0.00569301140274165;0.00254312253843751;2.06500815466155;0.00254309313259719]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395759753156;-9.97359339940187;-6.64819520364775]
Robot Arm 1 Joint Angles: [-0.00246129030556191;1.869343452737;-0.0024612903055562;-0.00729550261551014;-0.00246129030557468;-1.78347029823298;-0.00246128959256398]
Mobile Platform 2 Pose: [-2.35180295259886;9.97433278837352;6.415849498617]
Robot Arm 2 Joint Angles: [0.00254327041981094;-1.82062906439392;0.00254327041977852;0.00569285326029894;0.00254329869540811;1.79123429155989;0.00254326928956779]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785395759655636;-9.97198118931604;-6.65650177558254]
Robot Arm 1 Joint Angles: [-0.00246139016627963;2.12729682409647;-0.0024613901662739;-0.0072952602415042;-0.00246139016629238;-2.04142415434046;-0.00246138945328168]
Mobile Platform 2 Pose: [-2.35180295249154;9.9724172939362;6.41259389907407]
Robot Arm 2 Joint Angles: [0.00254338030616716;-2.12710807322308;0.00254338030613475;0.00569275211758334;0.00254340858176435;2.09771350267448;0.00254337917592403]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395759597111;-9.97295374746973;-6.65125334903681]
Robot Arm 1 Joint Angles: [-0.00246145009563331;1.97168737727154;-0.00246145009562759;-0.00729511800759162;-0.00246145009564607;-1.88581499198336;-0.00246144938263537]
Mobile Platform 2 Pose: [-2.35180295242785;9.97397271070488;6.41511520265248]
Robot Arm 2 Joint Angles: [0.00254344552764445;-1.87824133184433;0.00254344552761203;0.00569269372728901;0.00254347380324163;1.84884687807632;0.00254344439740131]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785395759561427;-9.97296922404441;-6.6513492560343]
Robot Arm 1 Joint Angles: [-0.00246148663562581;1.96921103745212;-0.0024614866356201;-0.00729503013686692;-0.00246148663563858;-1.88333882790539;-0.00246148592262787]
Mobile Platform 2 Pose: [-2.35180295238884;9.97336147482351;6.41385488180016]
Robot Arm 2 Joint Angles: [0.00254348546983034;-1.97603903630121;0.00254348546979792;0.00569265716578571;0.00254351374542752;1.94664465565621;0.0025434843395872]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785395759539366;-9.97196650325642;-6.6562907865685]
Robot Arm 1 Joint Angles: [-0.00246150922616748;2.1296463091937;-0.00246150922616176;-0.00729497579976558;-0.00246150922618023;-2.04377420832117;-0.00246150851316953]
Mobile Platform 2 Pose: [-2.35180295236504;9.97289738267509;6.41328889313085]
Robot Arm 2 Joint Angles: [0.00254350984675688;-2.05029375793716;0.00254350984672447;0.00569263505361612;0.00254353812235406;2.0208994215165;0.00254350871651374]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.78539575952626;-9.97359580921727;-6.64798611087732]
Robot Arm 1 Joint Angles: [-0.00246152264733956;1.86895732361144;-0.00246152264733385;-0.0072949439527453;-0.00246152264735233;-1.78308528643295;-0.00246152193434162]
Mobile Platform 2 Pose: [-2.35180295235089;9.97424203820227;6.41528391680531]
Robot Arm 2 Joint Angles: [0.00254352433200937;-1.83514886053418;0.00254352433197695;0.00569262200060446;0.00254355260760654;1.80575455021954;0.00254352320176622]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785395759518025;-9.97165958807069;-6.65794808034011]
Robot Arm 1 Joint Angles: [-0.00246153108011986;2.178752686596;-0.00246153108011414;-0.00729492348487617;-0.00246153108013261;-2.09288069035325;-0.00246153036712191]
Mobile Platform 2 Pose: [-2.35180295234206;9.97233246730113;6.41225119261676]
Robot Arm 2 Joint Angles: [0.00254353337589538;-2.14068019640479;0.00254353337586297;0.00569261368809108;0.00254356165149257;2.11128590271518;0.00254353224565225]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785395759513141;-9.97340627578782;-6.64887127655015]
Robot Arm 1 Joint Angles: [-0.00246153608088411;1.89928264003497;-0.0024615360808784;-0.00729491166461823;-0.00246153608089688;-1.81341066743274;-0.00246153536788618]
Mobile Platform 2 Pose: [-2.35180295233685;9.97417848563497;6.41526075960781]
Robot Arm 2 Joint Angles: [0.00254353871660615;-1.84531725821373;0.00254353871657373;0.00569260891215677;0.00254356699220332;1.81592297407599;0.002543537586363]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785395759510112;-9.97238834370791;-6.65431341078414]
Robot Arm 1 Joint Angles: [-0.00246153918221748;2.06215176531351;-0.00246153918221176;-0.00729490415802862;-0.00246153918223023;-1.97627980772445;-0.00246153846921953]
Mobile Platform 2 Pose: [-2.35180295233362;9.97303054401092;6.4132147935521]
Robot Arm 2 Joint Angles: [0.0025435420160689;-2.02898791503254;0.00254354201603649;0.00569260588248185;0.00254357029166608;1.99959363695414;0.00254354088582576]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785395759508273;-9.97251220481971;-6.6534707095392]
Robot Arm 1 Joint Angles: [-0.00246154106582999;2.04233398293087;-0.00246154106582428;-0.00729489966353731;-0.00246154106584275;-1.9564620343308;-0.00246154035283205]
Mobile Platform 2 Pose: [-2.35180295233168;9.97322032496402;6.41371997255738]
Robot Arm 2 Joint Angles: [0.00254354401028731;-1.99862296073686;0.0025435440102549;0.00569260408208842;0.00254357228588449;1.96922868625925;0.00254354288004417]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785395759507166;-9.97332888203844;-6.64945225394588]
Robot Arm 1 Joint Angles: [-0.00246154219933972;1.91166562522433;-0.00246154219933401;-0.00729489695433561;-0.00246154219935249;-1.82579368204266;-0.00246154148634179]
Mobile Platform 2 Pose: [-2.35180295233051;9.97406840253887;6.41488917819979]
Robot Arm 2 Joint Angles: [0.00254354520401941;-1.86293054768009;0.00254354520398699;0.00569260300146647;0.00254357347961658;1.83353627536372;0.00254354407377626]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785395759506474;-9.97169930686996;-6.65771324223667]
Robot Arm 1 Joint Angles: [-0.00246154290748153;2.1723976504694;-0.00246154290747581;-0.00729489524224964;-0.00246154290749428;-2.08652571071191;-0.00246154219448358]
Mobile Platform 2 Pose: [-2.35180295232979;9.97236674509821;6.41227481378582]
Robot Arm 2 Joint Angles: [0.00254354594672896;-2.13519573750505;0.00254354594669655;0.00569260232063337;0.00254357422232614;2.10580146655035;0.00254354481648582]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785395759506065;-9.97361778204734;-6.64785910460435]
Robot Arm 1 Joint Angles: [-0.00246154332633321;1.86544162109776;-0.0024615433263275;-0.00729489425172447;-0.00246154332634598;-1.77956968332132;-0.00246154261333528]
Mobile Platform 2 Pose: [-2.35180295232936;9.97430432976991;6.41537986253557]
Robot Arm 2 Joint Angles: [0.00254354638461304;-1.82518218964095;0.00254354638458062;0.00569260192887867;0.00254357466021022;1.79578791946976;0.0025435452543699]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785395759505809;-9.97187540409097;-6.656938911363]
Robot Arm 1 Joint Angles: [-0.00246154358894332;2.14422209348063;-0.00246154358893759;-0.00729489361411645;-0.00246154358895607;-2.05835015697939;-0.00246154287594537]
Mobile Platform 2 Pose: [-2.35180295232909;9.97272914125367;6.41270478745739]
Robot Arm 2 Joint Angles: [0.0025435466568978;-2.0772123519895;0.00254354665686539;0.00569260167837304;0.00254357493249498;2.04781808231932;0.00254354552665466]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785395759505656;-9.97307917302321;-6.65059553927168]
Robot Arm 1 Joint Angles: [-0.00246154374588661;1.95161906394996;-0.00246154374588089;-0.00729489324214186;-0.00246154374589937;-1.86574712819268;-0.00246154303288867]
Mobile Platform 2 Pose: [-2.35180295232893;9.97354973181868;6.41419644688273]
Robot Arm 2 Joint Angles: [0.00254354681999444;-1.94591786144024;0.00254354681996202;0.00569260153174447;0.00254357509559162;1.91652359206332;0.0025435456897513]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785395759505562;-9.97283566172529;-6.65207568829744]
Robot Arm 1 Joint Angles: [-0.00246154384200664;1.99058087138579;-0.00246154384200093;-0.00729489301058375;-0.0024615438420194;-1.90470893609162;-0.0024615431290087]
Mobile Platform 2 Pose: [-2.35180295232884;9.97382047873738;6.41440944275528]
Robot Arm 2 Joint Angles: [0.00254354691851256;-1.90259835435893;0.00254354691848014;0.00569260144213338;0.00254357519410973;1.87320408516123;0.00254354578826941]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036872e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782864e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785395759505504;-9.97208304294275;-6.65574885789258]
Robot Arm 1 Joint Angles: [-0.00246154390119389;2.11099987644941;-0.00246154390118817;-0.00729489286848789;-0.00246154390120664;-2.02512794143944;-0.00246154318819594]
Mobile Platform 2 Pose: [-2.35180295232878;9.9725148564889;6.41248754992436]
Robot Arm 2 Joint Angles: [0.00254354697941832;-2.11149791406067;0.0025435469793859;0.00569260138651069;0.0025435752550155;2.08210364497421;0.00254354584917518]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.811171e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.140208e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.78539575950547;-9.97355589919412;-6.64829011432792]
Robot Arm 1 Joint Angles: [-0.00246154393643776;1.87534287614746;-0.00246154393643205;-0.00729489278473175;-0.00246154393645053;-1.78947094130499;-0.00246154322343983]
Mobile Platform 2 Pose: [-2.35180295232874;9.9743437771736;6.41536014653399]
Robot Arm 2 Joint Angles: [0.0025435470153681;-1.8188706044761;0.00254354701533568;0.00569260135428468;0.00254357529096528;1.7894763354541;0.00254354588512496]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563931e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.131768e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785395759505448;-9.97163424244322;-6.65815397495836]
Robot Arm 1 Joint Angles: [-0.0024615439585739;2.18280795623652;-0.00246154395856817;-0.00729489273103497;-0.00246154395858665;-2.09693602150146;-0.00246154324557594]
Mobile Platform 2 Pose: [-2.35180295232872;9.97249486404993;6.41231748256355]
Robot Arm 2 Joint Angles: [0.00254354703780921;-2.11469670424286;0.0025435470377768;0.00569260133362589;0.00254357531340639;2.08530243526218;0.00254354590756607]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056079e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.164758e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785395759505435;-9.97348102378628;-6.64859180451582]
Robot Arm 1 Joint Angles: [-0.00246154397168094;1.88732294131718;-0.00246154397167523;-0.0072948927000638;-0.00246154397169371;-1.80145100664405;-0.00246154325868301]
Mobile Platform 2 Pose: [-2.35180295232871;9.97384890639331;6.41462249990017]
Robot Arm 2 Joint Angles: [0.00254354705115196;-1.89804992928978;0.00254354705111954;0.00569260132166845;0.00254357532674914;1.86865566033302;0.00254354592090882]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231848e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.658943e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785395759505427;-9.97225080614539;-6.65511445526811]
Robot Arm 1 Joint Angles: [-0.00246154397983688;2.08415776383889;-0.00246154397983116;-0.00729489268030123;-0.00246154397984964;-1.99828582920529;-0.00246154326683894]
Mobile Platform 2 Pose: [-2.3518029523287;9.97351673349928;6.41385805520022]
Robot Arm 2 Joint Angles: [0.00254354705928982;-1.95119759232744;0.00254354705925741;0.0056926013142333;0.002543575334887;1.92180332338554;0.00254354592904668]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531990e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402575e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785395759505423;-9.97265434030515;-6.65284295629283]
Robot Arm 1 Joint Angles: [-0.00246154398476928;2.01959229826642;-0.00246154398476357;-0.00729489266855363;-0.00246154398478204;-1.93372036365632;-0.00246154327177134]
Mobile Platform 2 Pose: [-2.35180295232869;9.97275759405467;6.41284456630703]
Robot Arm 2 Joint Angles: [0.00254354706427826;-2.07265990345924;0.00254354706424585;0.00569260130969803;0.00254357533987544;2.04326563452642;0.00254354593403512]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692260e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.092528e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.78539575950542;-9.97322813797382;-6.65009145689996]
Robot Arm 1 Joint Angles: [-0.00246154398774938;1.92778467127224;-0.00246154398774367;-0.00729489266141704;-0.00246154398776215;-1.84191273667641;-0.00246154327475145]
Mobile Platform 2 Pose: [-2.35180295232869;9.9742953395649;6.4151932005821]
Robot Arm 2 Joint Angles: [0.00254354706723407;-1.8266206218205;0.00254354706720166;0.0056926013070397;0.00254357534283125;1.79722635289299;0.00254354593699093]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663859e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.038128e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.785395759505418;-9.97176377613675;-6.65747813759201]
Robot Arm 1 Joint Angles: [-0.00246154398960598;2.16208256519771;-0.00246154398960025;-0.00729489265693481;-0.00246154398961873;-2.07621063061084;-0.00246154327660803]
Mobile Platform 2 Pose: [-2.35180295232869;9.97235778798303;6.41208906064124]
Robot Arm 2 Joint Angles: [0.00254354706908093;-2.13662887491736;0.00254354706904852;0.00569260130534072;0.00254357534467812;2.10723460599324;0.0025435459388378]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984807e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.086454e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785395759505417;-9.97362810088841;-6.64792805207277]
Robot Arm 1 Joint Angles: [-0.00246154399070493;1.86379060493077;-0.00246154399069922;-0.0072948926543339;-0.0024615439907177;-1.7779186703491;-0.002461543277707]
Mobile Platform 2 Pose: [-2.35180295232869;9.97408986982989;6.41494898646442]
Robot Arm 2 Joint Angles: [0.00254354707017324;-1.85949577941887;0.00254354707014083;0.00569260130436367;0.00254357534577042;1.83010151049671;0.0025435459399301]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059253e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.365654e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785395759505416;-9.97178688319742;-6.65748565585436]
Robot Arm 1 Joint Angles: [-0.0024615439913949;2.15838543548645;-0.00246154399138918;-0.00729489265265837;-0.00246154399140765;-2.07251350090813;-0.00246154327839695]
Mobile Platform 2 Pose: [-2.35180295232869;9.97318617785564;6.41328278248208]
Robot Arm 2 Joint Angles: [0.00254354707084554;-2.00408649529897;0.00254354707081313;0.00569260130374714;0.00254357534644272;1.97469222637805;0.0025435459406024]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857436e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.453916e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785395759505416;-9.97319545975326;-6.65010797578431]
Robot Arm 1 Joint Angles: [-0.00246154399180599;1.93301318655206;-0.00246154399180028;-0.00729489265168518;-0.00246154399181875;-1.84714125197569;-0.00246154327880805]
Mobile Platform 2 Pose: [-2.35180295232869;9.97306379624314;6.41329591572629]
Robot Arm 2 Joint Angles: [0.00254354707125373;-2.02366755329791;0.00254354707122132;0.0056926013033778;0.00254357534685091;1.99427328437773;0.00254354594101059]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905819e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.897840e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785395759505415;-9.97269674450398;-6.65290069318509]
Robot Arm 1 Joint Angles: [-0.00246154399205884;2.01280762643513;-0.00246154399205312;-0.00729489265107502;-0.0024615439920716;-1.92693569185998;-0.0024615432790609]
Mobile Platform 2 Pose: [-2.35180295232869;9.97416079326396;6.41488649345469]
Robot Arm 2 Joint Angles: [0.00254354707149714;-1.84814802996678;0.00254354707146472;0.005692601303158;0.00254357534709432;1.81875376104704;0.002543545941254]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634522e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.328400e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785395759505415;-9.97221078172521;-6.65519569974022]
Robot Arm 1 Joint Angles: [-0.00246154399221388;2.09056167103909;-0.00246154399220816;-0.00729489265070351;-0.00246154399222663;-2.00468973646468;-0.00246154327921593]
Mobile Platform 2 Pose: [-2.35180295232869;9.97233564230596;6.41204152539317]
Robot Arm 2 Joint Angles: [0.00254354707164892;-2.14017218324579;0.00254354707161651;0.00569260130301859;0.0025435753472461;2.11077791432632;0.00254354594140578]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.067005e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.828058e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735006134901;-6.64870124197991]
Robot Arm 1 Joint Angles: [-0.00246154399230644;1.8841885886564;-0.00246154399230073;-0.0072948926504832;-0.00246154399231921;-1.79831665408243;-0.00246154327930851]
Mobile Platform 2 Pose: [-2.35180295232869;9.9742551552975;6.41514575943906]
Robot Arm 2 Joint Angles: [0.00254354707173842;-1.83305010459929;0.002543547071706;0.00569260130293863;0.00254357534733559;1.80365583567998;0.00254354594149527]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.069100e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.488247e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163094321734;-6.65826420090662]
Robot Arm 1 Joint Angles: [-0.00246154399236456;2.18333583229786;-0.00246154399235883;-0.00729489265034229;-0.00246154399237731;-2.09746389772418;-0.0024615432793666]
Mobile Platform 2 Pose: [-2.35180295232869;9.97286598665443;6.41274181893463]
Robot Arm 2 Joint Angles: [0.00254354707179393;-2.05531708749178;0.00254354707176152;0.00569260130288754;0.00254357534739111;2.02592281857257;0.00254354594155079]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.776572e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.703517e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785395759505415;-9.97354092061521;-6.64840078543374]
Robot Arm 1 Joint Angles: [-0.00246154399239889;1.87773944863744;-0.00246154399239318;-0.00729489265026122;-0.00246154399241166;-1.79186751406391;-0.00246154327940096]
Mobile Platform 2 Pose: [-2.35180295232869;9.97339511960907;6.41378157210914]
Robot Arm 2 Joint Angles: [0.00254354707182731;-1.97065581474892;0.0025435470717949;0.00569260130285749;0.00254357534742449;1.94126154582978;0.00254354594158417]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247228e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.540963e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.785395759505415;-9.97212038216775;-6.6558824117393]
Robot Arm 1 Joint Angles: [-0.00246154399242036;2.10502560023202;-0.00246154399241463;-0.00729489265020912;-0.00246154399243311;-2.0191536656586;-0.00246154327942241]
Mobile Platform 2 Pose: [-2.35180295232869;9.97394646288785;6.41445748996217]
Robot Arm 2 Joint Angles: [0.00254354707184739;-1.88244089014391;0.00254354707181497;0.0056926013028393;0.00254357534744457;1.8530466212248;0.00254354594160425]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064539e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.494647e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785395759505415;-9.97279446002223;-6.65223019561338]
Robot Arm 1 Joint Angles: [-0.00246154399243326;1.99717314351447;-0.00246154399242755;-0.00729489265017845;-0.00246154399244602;-1.91130120894111;-0.00246154327943532]
Mobile Platform 2 Pose: [-2.35180295232869;9.9724312993398;6.41217558468242]
Robot Arm 2 Joint Angles: [0.00254354707185985;-2.12486705783207;0.00254354707182744;0.00569260130282783;0.00254357534745703;2.09547278891299;0.00254354594161671]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.742991e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.506090e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.785395759505415;-9.97311524782296;-6.65079308472694]
Robot Arm 1 Joint Angles: [-0.00246154399244109;1.94584709539847;-0.00246154399243538;-0.00729489265015967;-0.00246154399245386;-1.85997516082515;-0.00246154327944316]
Mobile Platform 2 Pose: [-2.35180295232869;9.9743360175602;6.41519762147065]
Robot Arm 2 Joint Angles: [0.00254354707186718;-1.82011214256858;0.00254354707183476;0.00569260130282135;0.00254357534746436;1.79071787364951;0.00254354594162404]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155980e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.140272e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785395759505415;-9.97184705328127;-6.6571492964578]
Robot Arm 1 Joint Angles: [-0.00246154399244598;2.14875822206857;-0.00246154399244025;-0.00729489265014786;-0.00246154399245873;-2.06288628749528;-0.00246154327944803]
Mobile Platform 2 Pose: [-2.35180295232869;9.97259551863678;6.41229329957341]
Robot Arm 2 Joint Angles: [0.00254354707187177;-2.09859197031481;0.00254354707183936;0.00569260130281705;0.00254357534746895;2.06919770139574;0.00254354594162863]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302894e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.521174e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362257766344;-6.64808091524349]
Robot Arm 1 Joint Angles: [-0.00246154399244884;1.86467432092128;-0.00246154399244313;-0.00729489265014114;-0.00246154399246161;-1.778802386348;-0.0024615432794509]
Mobile Platform 2 Pose: [-2.35180295232869;9.97371275155802;6.41424009739768]
Robot Arm 2 Joint Angles: [0.00254354707187449;-1.91983470291596;0.00254354707184208;0.00569260130281465;0.00254357534747167;1.8904404339969;0.00254354594163135]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531498e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446492e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.785395759505415;-9.97171662409789;-6.6579385588234]
Robot Arm 1 Joint Angles: [-0.00246154399245068;2.16962689140944;-0.00246154399244495;-0.00729489265013662;-0.00246154399246343;-2.08375495683617;-0.00246154327945273]
Mobile Platform 2 Pose: [-2.35180295232869;9.97366623354717;6.4139358472443]
Robot Arm 2 Joint Angles: [0.00254354707187615;-1.92727758465212;0.00254354707184374;0.00569260130281314;0.00254357534747333;1.89788331573307;0.00254354594163301]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.613167e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.280278e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785395759505415;-9.97330001676718;-6.64968226679352]
Robot Arm 1 Joint Angles: [-0.00246154399245173;1.91628406432255;-0.00246154399244602;-0.00729489265013417;-0.0024615439924645;-1.83041212974928;-0.0024615432794538]
Mobile Platform 2 Pose: [-2.35180295232869;9.97263237226785;6.41246994506917]
Robot Arm 2 Joint Angles: [0.00254354707187718;-2.09269538934366;0.00254354707184477;0.00569260130281216;0.00254357534747436;2.06330112042461;0.00254354594163404]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408568e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.794392e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785395759505415;-9.97255494590405;-6.6537371298561]
Robot Arm 1 Joint Angles: [-0.00246154399245241;2.03549540242296;-0.00246154399244669;-0.00729489265013252;-0.00246154399246517;-1.9496234678497;-0.00246154327945446]
Mobile Platform 2 Pose: [-2.35180295232869;9.97432926165904;6.41510044213069]
Robot Arm 2 Joint Angles: [0.00254354707187777;-1.82119308675373;0.00254354707184535;0.0056926013028117;0.00254357534747495;1.79179881783468;0.00254354594163463]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394164e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.099122e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785395759505415;-9.97234650754405;-6.65460270477929]
Robot Arm 1 Joint Angles: [-0.00246154399245282;2.06884554002401;-0.0024615439924471;-0.00729489265013153;-0.00246154399246557;-1.98297360545075;-0.00246154327945487]
Mobile Platform 2 Pose: [-2.35180295232869;9.97240927489286;6.41198570405513]
Robot Arm 2 Joint Angles: [0.00254354707187816;-2.12839096934222;0.00254354707184575;0.00569260130281126;0.00254357534747534;2.09899670042317;0.00254354594163502]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956357e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.353657e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785395759505415;-9.97343026926908;-6.64918949930142]
Robot Arm 1 Joint Angles: [-0.00246154399245304;1.89544366401808;-0.00246154399244733;-0.00729489265013102;-0.00246154399246581;-1.80957172944482;-0.00246154327945511]
Mobile Platform 2 Pose: [-2.35180295232869;9.9739841467385;6.41461871208543]
Robot Arm 2 Joint Angles: [0.00254354707187837;-1.87641147403953;0.00254354707184596;0.00569260130281114;0.00254357534747555;1.84701720512047;0.00254354594163523]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260469e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.096596e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164968951921;-6.65826235962286]
Robot Arm 1 Joint Angles: [-0.00246154399245322;2.18033642399812;-0.0024615439924475;-0.00729489265013054;-0.00246154399246597;-2.09446448942486;-0.00246154327945527]
Mobile Platform 2 Pose: [-2.35180295232869;9.97334416463998;6.41336509774369]
Robot Arm 2 Joint Angles: [0.00254354707187852;-1.97880860980345;0.0025435470718461;0.00569260130281098;0.0025435753474757;1.9494143408844;0.00254354594163538]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807923e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.758885e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785395759505415;-9.97358545598195;-6.64829065134618]
Robot Arm 1 Joint Angles: [-0.00246154399245328;1.87061378995892;-0.00246154399244758;-0.00729489265013045;-0.00246154399246605;-1.78474185538566;-0.00246154327945535]
Mobile Platform 2 Pose: [-2.35180295232869;9.9729129342803;6.41288356651987]
Robot Arm 2 Joint Angles: [0.0025435470718786;-2.04780546735243;0.00254354707184619;0.00569260130281088;0.00254357534747578;2.01841119843338;0.00254354594163546]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620323e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.152128e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.785395759505415;-9.97200013539397;-6.65659579684792]
Robot Arm 1 Joint Angles: [-0.00246154399245336;2.12426508403601;-0.00246154399244764;-0.00729489265013021;-0.00246154399246612;-2.03839314946275;-0.00246154327945542]
Mobile Platform 2 Pose: [-2.35180295232869;9.97423512482072;6.41485853728079]
Robot Arm 2 Joint Angles: [0.00254354707187864;-1.83625498088515;0.00254354707184623;0.0056926013028109;0.00254357534747582;1.8068607119661;0.0025435459416355]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640783e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.565881e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785395759505415;-9.97292968713781;-6.65164343752433]
Robot Arm 1 Joint Angles: [-0.00246154399245338;1.9755368050224;-0.00246154399244767;-0.00729489265013019;-0.00246154399246615;-1.88966487044914;-0.00246154327945544]
Mobile Platform 2 Pose: [-2.35180295232869;9.9723312913972;6.41185089849531]
Robot Arm 2 Joint Angles: [0.00254354707187869;-2.14086832864768;0.00254354707184627;0.00569260130281078;0.00254357534747587;2.11147405972863;0.00254354594163555]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711110e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.989243e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785395759505415;-9.97299183541534;-6.65154595274172]
Robot Arm 1 Joint Angles: [-0.0024615439924534;1.96559308061636;-0.00246154399244769;-0.00729489265013015;-0.00246154399246617;-1.8797211460431;-0.00246154327945546]
Mobile Platform 2 Pose: [-2.35180295232869;9.97418689256759;6.41487975953684]
Robot Arm 2 Joint Angles: [0.00254354707187869;-1.8439721413858;0.00254354707184628;0.00569260130281086;0.00254357534747587;1.81457787246675;0.00254354594163555]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373557e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.265236e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785395759505415;-9.97194709741018;-6.65673575840169]
Robot Arm 1 Joint Angles: [-0.00246154399245343;2.13275116144264;-0.00246154399244771;-0.00729489265013005;-0.00246154399246618;-2.04687922686938;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.35180295232869;9.97301411970158;6.41280037256995]
Robot Arm 2 Joint Angles: [0.00254354707187871;-2.03161579994807;0.0025435470718463;0.00569260130281079;0.00254357534747589;2.00222153102902;0.00254354594163557]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767804e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.485721e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360123983821;-6.64831637142007]
Robot Arm 1 Joint Angles: [-0.00246154399245341;1.86808837295744;-0.00246154399244771;-0.00729489265013014;-0.00246154399246618;-1.78221643838418;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.35180295232869;9.97323731305587;6.41336075702814]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.99590486326146;0.0025435470718463;0.00569260130281079;0.0025435753474759;1.96651059434241;0.00254354594163558]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002526e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.098903e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166632921239;-6.65828886791101]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17767407308892;-0.00246154399244772;-0.00729489265013;-0.0024615439924662;-2.09180213851566;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97405746844036;6.41448468925427]
Robot Arm 2 Joint Angles: [0.00254354707187871;-1.86468000174217;0.0025435470718463;0.00569260130281083;0.00254357534747589;1.83528573282312;0.00254354594163557]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.187228e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.309803e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785395759505415;-9.97339148554344;-6.64932526492237]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90164906012155;-0.00246154399244771;-0.00729489265013011;-0.00246154399246619;-1.81577712554829;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35180295232869;9.9723716716565;6.4119001116723]
Robot Arm 2 Joint Angles: [0.00254354707187873;-2.1344074871609;0.00254354707184632;0.00569260130281074;0.00254357534747591;2.10501321824185;0.00254354594163559]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546799e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.844088e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785395759505415;-9.97241328591609;-6.65456938368534]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.05816100049619;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.97228906592293;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97430848198955;6.41500149115886]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.82451783387269;0.0025435470718463;0.00569260130281084;0.0025435753474759;1.79512356495364;0.00254354594163557]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.136121e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.367387e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724870348897;-6.65398600376929]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.04636116471989;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.96048923014663;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97271530647356;6.41230115649932]
Robot Arm 2 Joint Angles: [0.00254354707187873;-2.07942591643102;0.00254354707184632;0.00569260130281076;0.00254357534747591;2.05003164751197;0.00254354594163559]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235454e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.601905e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785395759505415;-9.97334546906772;-6.64975107323676]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90901169623616;-0.00246154399244772;-0.00729489265013009;-0.00246154399246619;-1.8231397616629;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35180295232869;9.9735660146079;6.41383947197072]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.94331261493714;0.00254354707184631;0.0056926013028108;0.0025435753474759;1.91391834601809;0.00254354594163558]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.018100e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.083674e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785395759505415;-9.97169001677896;-6.65815098765241]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17388406243769;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.08801212786443;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97380617666009;6.41400244383762]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.90488668658671;0.00254354707184631;0.00569260130281081;0.0025435753474759;1.87549241766766;0.00254354594163558]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924401e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.099173e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361436263305;-6.64826276794917]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.86598872578265;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.78011679120939;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35180295232869;9.97252518373261;6.4121223875144]
Robot Arm 2 Joint Angles: [0.00254354707187873;-2.1098455549821;0.00254354707184632;0.00569260130281075;0.00254357534747591;2.08045128606305;0.00254354594163559]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213735e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.272985e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.785395759505415;-9.97189296008914;-6.6572402648311]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.14141313280842;-0.00246154399244773;-0.00729489265013;-0.0024615439924662;-2.05554119823516;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97434346104515;6.4149749867398]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.8189211849759;0.0025435470718463;0.00569260130281084;0.0025435753474759;1.78952691605685;0.00254354594163558]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673361e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.746119e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785395759505415;-9.97305746965453;-6.65109586276226]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.9550916023463;-0.00246154399244772;-0.00729489265013007;-0.0024615439924662;-1.86921966777304;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97248535393241;6.41192157423286]
Robot Arm 2 Joint Angles: [0.00254354707187873;-2.11621832301495;0.00254354707184632;0.00569260130281074;0.00254357534747591;2.0868240540959;0.00254354594163559]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183064e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.588255e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.785395759505415;-9.97285991725431;-6.65233929974818]
Robot Arm 1 Joint Angles: [-0.00246154399245344;1.98669998638172;-0.00246154399244772;-0.00729489265013006;-0.0024615439924662;-1.90082805180846;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97386280294803;6.41426170634046]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.89582648051617;0.00254354707184631;0.00569260130281081;0.0025435753474759;1.86643221159712;0.00254354594163558]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.344629e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.362363e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.785395759505415;-9.97206146908278;-6.65624996197738]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.11445169382607;-0.00246154399244773;-0.00729489265013001;-0.0024615439924662;-2.02857975925281;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97350028928111;6.4134487287726]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.95382866722352;0.00254354707184631;0.00569260130281079;0.0025435753474759;1.92443439830447;0.00254354594163558]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.852530e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.672029e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.785395759505415;-9.97356419826383;-6.64863342522325]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.87401502485864;-0.00246154399244772;-0.0072948926501301;-0.00246154399246619;-1.78814309028538;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35180295232869;9.97277198278717;6.4124856053873]
Robot Arm 2 Joint Angles: [0.00254354707187873;-2.07035770625404;0.00254354707184632;0.00569260130281076;0.00254357534747591;2.04096343733498;0.00254354594163559]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.176508e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.796372e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163721579333;-6.65853078357501]
Robot Arm 1 Joint Angles: [-0.00246154399245346;2.18233222013868;-0.00246154399244773;-0.00729489265012998;-0.0024615439924662;-2.09646028556542;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35180295232869;9.97429056862947;6.41480072491345]
Robot Arm 2 Joint Angles: [0.00254354707187872;-1.82738397148456;0.0025435470718463;0.00569260130281083;0.0025435753474759;1.79798970256551;0.00254354594163558]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.271319e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.465138e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.785395759505415;-9.97346885736037;-6.64904234075487]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.88926956941166;-0.00246154399244772;-0.0072948926501301;-0.00246154399246619;-1.8033976348384;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35180295232869;9.9723538247593;6.41170204068531]
Robot Arm 2 Joint Angles: [0.00254354707187873;-2.13726299071233;0.00254354707184632;0.00569260130281074;0.00254357534747591;2.10786872179327;0.00254354594163559]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.031279e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.232754e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.785395759505415;-9.97227493068446;-6.65538143162157]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.08029783755767;-0.00246154399244772;-0.00729489265013002;-0.0024615439924662;-1.99442590298441;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35000203693407;9.97410024200656;6.41458210929533]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.95182175721358;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.82844196431391;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.041627e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.698843e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.785395759505415;-9.97262916365023;-6.65336195104755]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.02362056303374;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.93774862846048;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35000203693407;9.97316932697385;6.41287370966818]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.10076816244692;0.00254354893449146;0.00569259944016563;0.00254357721012106;1.97738836954726;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.761691e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.522621e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.785395759505415;-9.97324708416375;-6.65038084070059]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.92475328087144;-0.00246154399244772;-0.00729489265013008;-0.0024615439924662;-1.83888134629818;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35000203693407;9.97326247202345;6.4132321812402]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.08586495451065;0.00254354893449146;0.00569259944016563;0.00254357721012106;1.96248516161099;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.312919e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.111766e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.785395759505415;-9.97175093531609;-6.65793527848102]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.16413709649706;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.0782651619238;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.35000203693407;9.97403915095984;6.41428336629134]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.96159632468839;0.00254354893449145;0.00569259944016567;0.00254357721012105;1.83821653178873;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.280231e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.327068e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362749625136;-6.64831780135496]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.86388734685385;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.77801541228059;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.35000203693407;9.97247996882274;6.41192597235284]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.21106546662506;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.0876856737254;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.480564e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.573472e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.785395759505415;-9.97180145232356;-6.65780304260271]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.04932858054573;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.07018243855138;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97436321797115;6.41490371644689]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.90974560287908;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.78636580997941;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.487033e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.435166e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.785395759505415;-9.97317561660938;-6.65059885805685]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.82946229481433;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.85031615281998;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97251812602794;6.41186537900885]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.20496031379296;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.08158052089329;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.571945e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.177422e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.785395759505415;-9.97292499663906;-6.6521192981908]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.86956149006646;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.89041534807211;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97398574001325;6.41434156919448]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.97014207614228;0.00254354893449145;0.00569259944016567;0.00254357721012105;1.84676228324262;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.011727e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.788680e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.785395759505415;-9.97198077407348;-6.65678676689502]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.02063710055912;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.04149095856476;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97333783744176;6.41307553324586]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.07380648758177;0.00254354893449146;0.00569259944016564;0.00254357721012106;1.95042669468211;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.393300e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.574758e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.785395759505415;-9.97368215801135;-6.64811672902138]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.74841567049886;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.7692695285045;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97309510999234;6.41289989975262]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.11264287948845;0.00254354893449146;0.00569259944016562;0.00254357721012106;1.98926308658879;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.565075e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.386534e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.785395759505415;-9.97178504506682;-6.65793527790616]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.05195374162463;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.07280759963028;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97414518282567;6.41441093394393]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.94463122615508;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.82125143325542;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.799599e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.324923e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.785395759505415;-9.97320214971113;-6.65051458725476]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.82521699853451;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.84607085654016;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97241486688285;6.41174609015733]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.22148177700675;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.09810198410708;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.089842e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.425928e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.785395759505415;-9.97289429288193;-6.65232974607302]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.87447409120711;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.89532794921276;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97434734761736;6.41483284012289]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91228485948555;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.78890506658588;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956346e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.950631e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.785395759505415;-9.97200520943873;-6.65671092704471]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.01672744211814;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.03758130012378;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97262220631009;6.41194890747598]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.18830746864832;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.06492767574866;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.924605e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.762794e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.785395759505415;-9.97367772595555;-6.64819493833102]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.74912479942716;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.7699786574328;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97385365716428;6.41407116477818]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.99127533197797;0.00254354893449146;0.00569259944016566;0.00254357721012105;1.8678955390783;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.708709e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059090e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.785395759505415;-9.97176957889764;-6.65806265536356]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.05442832869264;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.07528218669828;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97350508378659;6.41328058921271]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.04704707240843;0.00254354893449146;0.00569259944016564;0.00254357721012105;1.92366727950876;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.304875e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.162586e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.785395759505415;-9.97322813672972;-6.6504331916039]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.82105907555962;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.84191293356527;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97293423814149;6.41257667031943]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.13838237562454;0.00254354893449146;0.00569259944016562;0.00254357721012106;2.01500258272488;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.576426e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.954170e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.785395759505415;-9.97286329039368;-6.65254156253431]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.87943448932517;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.90028834733082;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97423118171876;6.41450818368945]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.93087140326117;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.8074916103615;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183589e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402781e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.785395759505415;-9.9720303420191;-6.65663157008412]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.01270622925908;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.03356008726473;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97237965820541;6.41161163296344]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.22711516539754;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.10373537249787;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.083688e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.752976e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.785395759505415;-9.97367246624214;-6.64827744804719]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.74996635357155;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.7708202115772;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97430960228296;6.41472426352409]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91832411299025;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.79494432009059;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.319921e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.659326e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.785395759505415;-9.97175507343266;-6.65818507881624]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.05674920308878;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.07760306109443;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97274895877047;6.41207013050537]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.16802707498818;0.00254354893449147;0.00569259944016561;0.00254357721012106;2.04464728208851;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.481148e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.709911e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.785395759505415;-9.97325355978884;-6.65035476501958]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.81699138610084;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.83784524410649;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97370658957665;6.41377538152598]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.0148061459991;0.00254354893449146;0.00569259944016565;0.00254357721012105;1.89142635309944;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.280657e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.602682e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.785395759505415;-9.97283201772133;-6.65275459963107]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.88443811690103;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.90529197490668;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97366611643177;6.41348105085786]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.02128184918001;0.00254354893449146;0.00569259944016565;0.00254357721012105;1.89790205628035;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.616602e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.218852e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.785395759505415;-9.97205614530534;-6.65654883164533]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.00857770346032;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.02943156146596;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97278525898417;6.41227085150794]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.16221904079706;0.00254354893449147;0.00569259944016561;0.00254357721012106;2.0388392478974;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.066023e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.720234e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.785395759505415;-9.97366638039939;-6.64836424578275]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.75094008841263;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77179394641828;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97429617589693;6.41457240818241]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.92047233475503;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.79709254185537;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.278599e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.584497e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.785395759505415;-9.97174154707189;-6.65830245874061]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.05891342081249;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.07976727881813;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97237556770199;6.41152479842252]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.22776964594563;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.10438985304596;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.903239e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.267987e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.785395759505415;-9.97327840153693;-6.65027939852465]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.81301670640561;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.83387056441125;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9742502057598;6.41457829356199]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.92782755669612;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.80444776379645;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.689677e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.489507e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.785395759505415;-9.97280050462447;-6.65296870351143]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.88948021239855;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.91033407040419;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97289397895234;6.41222305901195]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.14482384588932;0.00254354893449147;0.00569259944016561;0.00254357721012106;2.02144405298966;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.199722e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.028739e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.785395759505415;-9.97208259185995;-6.65646285211967]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.00434625472205;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.02520011272769;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97354793451168;6.41345990253507]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.04019095639445;0.00254354893449146;0.00569259944016565;0.00254357721012105;1.91681116349479;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.630135e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.436265e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.785395759505415;-9.97365947028969;-6.64845531717689]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.75204570596467;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77289956397031;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97381658462077;6.41366969488966]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.99720693894033;0.00254354893449146;0.00569259944016566;0.00254357721012105;1.87382714604066;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.495996e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.844389e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.785395759505415;-9.97172901752585;-6.65841470910716]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.06091814817851;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08177200618415;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97265328762997;6.41199031893255]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.18333445746916;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.05995466456949;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.036728e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.643356e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.785395759505415;-9.97330264511986;-6.65020718061739]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.80913773313676;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.8299915911424;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97433961243485;6.41460165434418]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91352248868697;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.79014269578731;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.584190e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.814762e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.785395759505415;-9.97276878208059;-6.65318371439606]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.8945558194201;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.91540967742575;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97240273863528;6.41148625392488]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.22342229661917;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.10004250371951;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.925545e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.134538e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.785395759505415;-9.97210965325292;-6.65637377699094]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.00001643184743;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.02087028985307;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9741696245753;6.41439581393565]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.94072054621504;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.81734075331537;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.606244e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.407075e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.785395759505415;-9.97365173816318;-6.64855064561666]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.75328284620576;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.7741367042114;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97305227130574;6.4124007070005]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.11949706934576;0.00254354893449146;0.00569259944016562;0.00254357721012106;1.9961172764461;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.906425e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.838100e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.785395759505415;-9.97171750171896;-6.65852174787044]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.06276067728148;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08361453528713;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97338185760341;6.41313152054095]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.06676326171748;0.00254354893449146;0.00569259944016564;0.00254357721012106;1.94338346881781;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.682590e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531585e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.785395759505415;-9.97332627416462;-6.65013819733713]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.8053570859754;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.82621094398104;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97395288153297;6.41384015353786]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.97539943298853;0.00254354893449146;0.00569259944016567;0.00254357721012105;1.85201964008886;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.814094e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.101172e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.785395759505415;-9.97273688227077;-6.65339946665951]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.89965978899197;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.92051364699762;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97254290585933;6.41174212624163]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.20099554076975;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.07761574787008;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.657350e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.718252e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.785395759505415;-9.972137300009;-6.65628175710842]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.99559295087537;-0.00246154212980258;-0.00729489451277517;-0.00246154212982105;-2.01644680888102;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97436121367447;6.41459457236677]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91006629034778;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.78668649744811;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.874712e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.535141e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.785395759505415;-9.97364318671323;-6.6486502119514]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.7546510781973;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77550493620295;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97246022811801;6.41149512831513]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.21422397938225;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.09084418648259;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.461173e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.474616e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.785395759505415;-9.97170701567991;-6.65862349752351]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.06443844352882;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08529230153446;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9740687042399;6.41417845107039]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.95686779987907;0.00254354893449145;0.00569259944016567;0.00254357721012105;1.8334880069794;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.698208e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.399196e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.785395759505415;-9.97334927277402;-6.6500725322727]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.80167730847147;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.82253116647712;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97321848390445;6.4125953764231]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.09290305355101;0.00254354893449146;0.00569259944016563;0.00254357721012106;1.96952326065135;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.276328e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.405646e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.785395759505415;-9.97270483854452;-6.65361578901848]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.90478678519217;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.92564064319782;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97321317046148;6.41279792652039]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.09375320442585;0.00254354893449146;0.00569259944016563;0.00254357721012106;1.97037341152619;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.977073e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.993778e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.785395759505415;-9.97216550156991;-6.65618694888298]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.99108070112875;-0.00246154212980258;-0.00729489451277517;-0.00246154212982105;-2.01193455913439;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97407220130962;6.41398706299777]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.9563082687246;0.00254354893449146;0.00569259944016567;0.00254357721012105;1.83292847582494;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.351059e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.810317e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.785395759505415;-9.97363381913217;-6.64875399420928]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.75614989116732;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77700374917297;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97245796123542;6.41153223642474]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.21458668059596;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.0912068876963;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.090640e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.977255e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.785395759505415;-9.9716975744224;-6.65871988570061]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.06594904473114;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08680290273678;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97436086953792;6.41455029644847]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91012135219643;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.78674155929677;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.981183e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.170649e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.785395759505415;-9.97337162553168;-6.65001026651711]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.79810086724557;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.81895472525121;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97254603705617;6.41154904067475]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.20049454927628;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.07711475637662;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.999182e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.107011e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.785395759505415;-9.97267268536299;-6.65383250483035]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.90993129423651;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.93078515224216;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9739488238323;6.41392876021467]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.97604866509581;0.00254354893449146;0.00569259944016567;0.00254357721012105;1.85266887219615;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.730013e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.470136e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.785395759505415;-9.97219422627439;-6.65608951439104]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.98648474841173;-0.00246154212980258;-0.00729489451277517;-0.00246154212982105;-2.00733860641737;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97338717817625;6.4127990012731]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.0659119700633;0.00254354893449146;0.00569259944016564;0.00254357721012106;1.94253217716364;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.755031e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.415272e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362363916551;-6.64886196732504]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.75777868583372;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77863254383937;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97304712859;6.41246739282986]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.12031990386315;0.00254354893449146;0.00569259944016562;0.00254357721012106;1.99694011096349;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.442211e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.670909e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.785395759505415;-9.97168919181965;-6.65881084580956]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.0672902611701;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08814411917575;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97417250977707;6.41410610291436]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.9402589139318;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.81687912103214;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.457851e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.914916e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.785395759505415;-9.97339331751631;-6.64995147857418]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.79463014970585;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.8154840077115;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97240141508072;6.41136532483534]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.22363406534764;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.10025427244798;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.358538e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.514758e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.785395759505415;-9.97264045822051;-6.65404943250173]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.91508763703349;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.93594149503914;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97433858090687;6.41446837958546]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91368753316415;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.79030774026449;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.803364e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.552624e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.785395759505415;-9.97222344135836;-6.65598962137495]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.98181033497777;-0.00246154212980257;-0.00729489451277517;-0.00246154212982105;-2.00266419298342;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9726571766658;6.41164416821643]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.18271221173576;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.0593324188361;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.056582e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.949681e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361265116299;-6.64897410288792]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.75953676623645;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78039062424209;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97381203847228;6.41365038945348]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.99793432269844;0.00254354893449146;0.00569259944016566;0.00254357721012105;1.87455452979877;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.095646e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.644234e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.785395759505415;-9.97168188047688;-6.65889631767365]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.06846007601366;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.0893139340193;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9735531088534;6.41300352361853]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.03936306172044;0.00254354893449146;0.00569259944016565;0.00254357721012105;1.91598326882078;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.075116e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.565375e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.785395759505415;-9.97341433432387;-6.64989624422431]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.79126746049623;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.81212131850188;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97288917356203;6.41214838753453]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.14559270833931;0.00254354893449147;0.00569259944016562;0.00254357721012106;2.02221291543965;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.501972e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.481166e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.785395759505415;-9.97260819354505;-6.65426638600337]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.92024998510734;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.94110384311298;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97425244478769;6.41419393978741]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.92746931223367;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.804089519334;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.113170e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.455256e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.785395759505415;-9.97225311297701;-6.65588744313062]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.97706287599287;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.99791673399851;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97237523626216;6.41124464582892]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.2278226763178;0.00254354893449147;0.00569259944016559;0.00254357721012107;2.10444288341814;0.00254354780428075]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.114105e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.387117e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.785395759505415;-9.9736008601249;-6.64909036891779]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.76142333233053;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78227719033617;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97429446051606;6.41434879412516]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.92074679569403;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.79736700279436;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.500251e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.258995e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.785395759505415;-9.97167565160559;-6.65897624816245]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.06945669542034;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.09031055342598;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97278977578842;6.41177536026874]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.16149635211679;0.00254354893449147;0.00569259944016561;0.00254357721012106;2.03811655921712;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.946346e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.975953e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.785395759505415;-9.97343466209613;-6.64984463635805]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.78801501693488;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.80886887494053;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97366117746043;6.41334817724261]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.02207208459471;0.00254354893449146;0.00569259944016566;0.00254357721012105;1.89869229169504;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.193892e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.883290e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.785395759505415;-9.972575928579;-6.65448317548415]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.92541237967559;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.94626623768123;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9737114806194;6.4132012600707]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.01402357916005;0.00254354893449146;0.00569259944016566;0.00254357721012105;1.89064378626038;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.597839e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.510773e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.785395759505415;-9.97228320624989;-6.65578315827782]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.97224795233347;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.99310181033911;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97274465273681;6.41184916861281]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.168716040374;0.00254354893449147;0.00569259944016561;0.00254357721012106;2.04533624747434;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.713249e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.385428e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.785395759505415;-9.97358827174218;-6.64921072967702]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.76343747356619;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78429133157184;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97431117306441;6.41424810271999]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91807278795883;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.79469299505917;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.411773e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.599045e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.785395759505415;-9.97167051490403;-6.6590505917904]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.07027856766979;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.09113242567544;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97238033571016;6.41117195256692]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.22700676463789;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.10362697173823;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.886660e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.515194e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.785395759505415;-9.97345428755363;-6.64979672478728]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.78487494373434;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.80572880173999;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97422879128676;6.41419200136726]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.93125387238185;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.80787407948218;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.270351e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.526748e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.785395759505415;-9.97254370124224;-6.65469960797338]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.93056875355759;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.95142261156323;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97293923409048;6.41193630877415]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.13758302378664;0.00254354893449147;0.00569259944016562;0.00254357721012106;2.01420323088698;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.212106e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.205539e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.785395759505415;-9.97231368532896;-6.6556769504119]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.96737129968217;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.98822515768782;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97349987142555;6.4130281479305]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.04788105017637;0.00254354893449146;0.00569259944016565;0.00254357721012105;1.92450125727671;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.701153e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.612009e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357489242918;-6.64933514552379]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.76557816364748;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78643202165313;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.973858148295;6.41338521430795]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.99055675106314;0.00254354893449146;0.00569259944016566;0.00254357721012105;1.86717695816347;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.666683e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.911152e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166647844774;-6.65911931126296]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.07092440067665;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.0917782586823;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9726185499341;6.41157740505508]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.18889248880787;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.0655126959082;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.371002e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.368225e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.785395759505415;-9.97347319803125;-6.64975257604422]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.78184926731475;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.80270312532039;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97434823582986;6.41426682961313]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91214274548564;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.78876295258598;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.616987e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.177747e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.785395759505415;-9.97251154998017;-6.65491548815742]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.93571295548738;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.95656681349302;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9724165374815;6.41114746108674]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.22121448122422;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.09783468832456;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.226321e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.015916e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.785395759505415;-9.9723445134894;-6.65556900763995]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.96243879401058;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.98329265201623;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97414213545597;6.41399908403981]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.94511880530787;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.8217390124082;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.968599e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.216573e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735607293481;-6.64946357281161]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.76784425661864;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78869811462428;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97310042413084;6.41211978487728]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.11179261732879;0.00254354893449146;0.00569259944016563;0.00254357721012106;1.98841282442913;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155043e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.721737e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166354859391;-6.6591823779516]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.07139317728991;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.09224703529555;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97333249287585;6.41269738626786]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.07466161812718;0.00254354893449146;0.00569259944016564;0.00254357721012106;1.95128182522752;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.253326e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.210767e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.785395759505415;-9.97349138151428;-6.64971225317892]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.77893991002979;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.79979376803544;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97398973692802;6.41354930064399]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.96950256977997;0.00254354893449146;0.00569259944016567;0.00254357721012105;1.8461227768803;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.153847e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.602009e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.785395759505415;-9.97247951360001;-6.6551306192138]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.94083877631414;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.96169263431978;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9725152515813;6.41133985726788]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.20542022525637;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.08204043235671;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.048183e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.432039e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.785395759505415;-9.97237565324156;-6.65545952200852]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.95745643366467;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.97831029167031;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97436341353272;6.41424892239181]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.90971431302766;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.78633452012799;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.437418e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.266521e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735457904246;-6.64959596383779]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.77023448437832;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.79108834238397;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97248258427189;6.41116985295828]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.21064699476194;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.08726720186228;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.026497e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.726039e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166172990292;-6.65923977228105]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.07168416784816;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.09253802585381;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9740354790214;6.41377192202146]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.96218383484038;0.00254354893449146;0.00569259944016567;0.00254357721012105;1.83880404194072;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.427366e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.289359e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.785395759505415;-9.97350882667309;-6.64967581556576]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.77614868462073;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.79700254262637;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97326793825823;6.4123179429959]
Robot Arm 2 Joint Angles: [0.00254354893452387;-2.08499035694715;0.00254354893449146;0.00569259944016564;0.00254357721012106;1.96161056404748;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.249120e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.254777e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.785395759505415;-9.97244763109885;-6.65534480368382]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.94593997649927;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.96679383450491;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97316400872797;6.41236379231897]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.10161908178915;0.00254354893449146;0.00569259944016563;0.00254357721012106;1.97823928888949;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.404751e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.719511e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.785395759505415;-9.97240706646205;-6.65534868883352]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.9524303183869;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.97328417639254;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97410367269657;6.41368846031012]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.95127284681314;0.00254354893449145;0.00569259944016568;0.00254357721012105;1.82789305391347;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.051554e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.512521e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.785395759505415;-9.97353008435404;-6.6497322668418]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.77274745566826;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.79360131367391;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97243836007729;6.41114212972586]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.21772286589676;0.00254354893449147;0.00569259944016559;0.00254357721012106;2.09434307299709;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.592385e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.600909e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166102507977;-6.65929148401513]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.07179693955107;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.09265079755672;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.97435663194503;6.41419364174325]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.91079936705854;0.00254354893449145;0.00569259944016569;0.00254357721012105;1.78741957415887;0.00254354780428073]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.815657e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.660301e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.785395759505415;-9.97352552289449;-6.64964331872866]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.77347728919733;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.79433114720297;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.35000203693407;9.97257617776495;6.4112363151023]
Robot Arm 2 Joint Angles: [0.00254354893452388;-2.19567203587164;0.00254354893449147;0.0056925994401656;0.00254357721012106;2.07229224297197;0.00254354780428074]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.908482e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.570890e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.785395759505415;-9.97241594148781;-6.65555784436287]
Robot Arm 1 Joint Angles: [-0.0024615421298083;1.95101031426523;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.97186417227088;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.35000203693407;9.9739103862174;6.41351337673622]
Robot Arm 2 Joint Angles: [0.00254354893452387;-1.98219868348021;0.00254354893449146;0.00569259944016567;0.00254357721012105;1.85881889058054;0.00254354780428073]
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 75)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',75,0)">第 75 行</a>)
    real_time_2D_plot(human_poses{k}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,1)">85  </a>    [dx1] = mobile_robot_CBLF(mobile_robot1, target1, a1_G, obstacle_results1, D_min, V_max, dt);
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.768390e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 Joint Angles: [-0.000121114227683022;0.185441233024398;-0.000121114227681381;1.86264489701995e-09;-0.000121114227682768;-0.22518584865809;-0.00012111422768375]
Mobile Platform 2 Pose: [-2.35629375636307;9.99866771831951;9.81844390973364]
Robot Arm 2 Joint Angles: [0.000121114227682417;-0.185441233024398;0.000121114227682772;-1.13478349175017e-09;0.000121114227682772;0.213165194053605;0.000121114227682772]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.231499e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 Joint Angles: [-0.000242257227143282;0.370882466048795;-0.00024225722714405;-0.000118135763669368;-0.000242257227146825;-0.450777934956321;-0.000242257227154676]
Mobile Platform 2 Pose: [-2.35629363805053;9.9973316791975;9.63690091169029]
Robot Arm 2 Joint Angles: [0.0002422644004499;-0.370882466048795;0.000242264400450605;8.2441396037932e-05;0.000242264400450605;0.426849008910947;0.000242264400450605]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.884894e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 Joint Angles: [-0.000363286279799942;0.556323699073193;-0.000363286279798916;-0.000357282809229051;-0.000363286279802673;-0.676844213293337;-0.000363286279809542]
Mobile Platform 2 Pose: [-2.35629351973276;9.99599115896504;9.45545425143654]
Robot Arm 2 Joint Angles: [0.000363419939334541;-0.556323699073193;0.000363419939335595;0.000250017326481657;0.000363419939335595;0.641164668047499;0.000363419939335595]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.695815e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 Joint Angles: [-0.000483866411480727;0.74176493209759;-0.000483866411478066;-0.000719510076232192;-0.000483866411482804;-0.903917115513842;-0.000483866411488692]
Mobile Platform 2 Pose: [-2.35629340160252;9.99464273537811;9.27450986850017]
Robot Arm 2 Joint Angles: [0.000484383442541989;-0.74176493209759;0.000484383442543392;0.00050515559533476;0.000484383442543392;0.856657301568752;0.000484383442543392]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.639865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 Joint Angles: [-0.0006036745926073;0.927206165121988;-0.000603674592606945;-0.00120731922081321;-0.000603674592618552;-1.13251733087994;-0.000603674592617571]
Mobile Platform 2 Pose: [-2.35629328383602;9.99328288902594;9.09451173184421]
Robot Arm 2 Joint Angles: [0.000604974473142317;-0.927206165121988;0.000604974473144067;0.000851200107465631;0.000604974473144067;1.07388667130081;0.000604974473144067]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.699424e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346623]
Robot Arm 1 Joint Angles: [-0.000722796083224335;1.11264739814639;-0.00072279608322361;-0.00182553790636024;-0.000722796083242086;-1.36238137284088;-0.000722796083234236]
Mobile Platform 2 Pose: [-2.3562931662462;9.99191235994051;8.91540928004993]
Robot Arm 2 Joint Angles: [0.00072538459509261;-1.11264739814639;0.000725384595094705;0.00129364145978263;0.000725384595094705;1.29272888152631;0.000725384595094705]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528709;-9.99002831581144;-8.79949883355178]
Robot Arm 1 Joint Angles: [-0.000841580556365119;1.29808863117078;-0.000841580556358477;-0.00257974514714843;-0.000841580556377935;-1.59288971083287;-0.000841580556369103]
Mobile Platform 2 Pose: [-2.3562930483992;9.99053428393946;8.73682080569227]
Robot Arm 2 Joint Angles: [0.000846058061965217;-1.29808863117078;0.000846058061967657;0.00183860210900188;0.000846058061967657;1.51267607896186;0.000846058061967657]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274173]
Robot Arm 1 Joint Angles: [-0.000959829987060112;1.48352986419518;-0.000959829987047145;-0.00347025803777765;-0.000959829987065621;-1.82450740028498;-0.00095982998705679]
Mobile Platform 2 Pose: [-2.35629293027947;9.98914654819751;8.55900547655282]
Robot Arm 2 Joint Angles: [0.000967010800718591;-1.48352986419518;0.000967010800721373;0.00248884074267602;0.000967010800721373;1.73406355696943;0.000967010800721373]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196129;-9.98711511781;-8.46062525935787]
Robot Arm 1 Joint Angles: [-0.00107743739451576;1.66897109721958;-0.0010774373945052;-0.00449854422257743;-0.00107743739452368;-2.05708258800973;-0.00107743739451485]
Mobile Platform 2 Pose: [-2.3561150781963;9.98774923908905;8.38210544558238]
Robot Arm 2 Joint Angles: [0.00108833623035836;-1.66897109721958;0.00108833623036151;0.00324800716781155;0.00108833623036151;1.95687373360414;0.00108833623036151]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929674;-9.98565931998688;-8.29072857539468]
Robot Arm 1 Joint Angles: [-0.00119546838174874;1.85441233024398;-0.00119546838174059;-0.00567665166884611;-0.00119546838175906;-2.0943951023932;-0.00119546838175023]
Mobile Platform 2 Pose: [-2.35593679035179;9.98635048448446;8.20503364444677]
Robot Arm 2 Joint Angles: [0.00121097369091691;-1.85441233024398;0.0012109736909173;0.00412545142017103;0.00121097369092022;2.1292566955194;0.00121097369092022]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.9841823282427;-8.12462828169497]
Robot Arm 1 Joint Angles: [-0.00131696123650541;2.03985356326837;-0.00131696123649832;-0.00639922083539073;-0.00131696123651679;-2.30399143679527;-0.00131696052350609]
Mobile Platform 2 Pose: [-2.35579619357455;9.98493653665738;8.02986222893095]
Robot Arm 2 Joint Angles: [0.00133403646749143;-2.03985356326837;0.00133403646749143;0.00495960992208546;0.00133403646749514;2.32955085759668;0.00133403698844777]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840344088;-7.9599482888563]
Robot Arm 1 Joint Angles: [-0.00143870942785493;2.14949927567626;-0.00143870942784766;-0.00719336673714352;-0.00143870942786614;-2.22826414941097;-0.00143870871485544]
Mobile Platform 2 Pose: [-2.35566970143879;9.98351892639362;7.85557397939221]
Robot Arm 2 Joint Angles: [0.00145818008620613;-2.14496046480796;0.00145819267407735;0.00583954858354825;0.00145818008621025;2.42129382496373;0.00145817897731786]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455234098;-7.79314279536571]
Robot Arm 1 Joint Angles: [-0.00156630119507468;2.18077941225243;-0.00156630119506725;-0.00742071448357543;-0.00156630119508573;-2.20427246494267;-0.00156630048207503]
Mobile Platform 2 Pose: [-2.35516363364112;9.98210890737395;7.679577860601]
Robot Arm 2 Joint Angles: [0.00158519384334056;-2.17750165774499;0.00158520643120731;0.00669299288292404;0.00158519384334502;2.28255168633733;0.00158519273445263]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.9797235609508;-7.63794018922978]
Robot Arm 1 Joint Angles: [-0.00168692949029622;2.19333238380943;-0.00168692949028909;-0.00746871767647478;-0.00168692949030756;-2.20035149691845;-0.00168692877729686]
Mobile Platform 2 Pose: [-2.35479825315092;9.98064949743592;7.51316394514502]
Robot Arm 2 Joint Angles: [0.00170708795535066;-2.18946883956983;0.00170710054321293;0.0069919545118042;0.00170708795535544;2.22080447038789;0.00170708684646305]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591665116;-7.46636918684545]
Robot Arm 1 Joint Angles: [-0.00182021652145274;2.1916426546638;-0.00182021652144546;-0.00746685601467987;-0.00182021652146393;-2.1937495063759;-0.00182021580845323]
Mobile Platform 2 Pose: [-2.35426984752474;9.9792859305255;7.33516238828991]
Robot Arm 2 Joint Angles: [0.001838877348049;-2.18839169373556;0.0018388899359068;0.00707201674727208;0.00183887734805413;2.19774434485739;0.00183887623916174]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346212;-9.97678818484225;-7.31817266015123]
Robot Arm 1 Joint Angles: [-0.00193743410109527;2.19689567720951;-0.00193743410108827;-0.0074507970691896;-0.00193743410110675;-2.19753736887274;-0.00193743338809605]
Mobile Platform 2 Pose: [-2.35345301987211;9.97780966086511;7.1759121845372]
Robot Arm 2 Joint Angles: [0.00195738274189135;-2.19345701983792;0.00195739532974465;0.00707867094215939;0.00195738274189678;2.19625882187523;0.0019573816330044]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232725;-9.9753590119579;-7.16418511934567]
Robot Arm 1 Joint Angles: [-0.00205940500907929;2.19373921813631;-0.00205940500907258;-0.00742961334509667;-0.00205940500909106;-2.19394443665204;-0.00205940429608036]
Mobile Platform 2 Pose: [-2.35224751800774;9.97642887561156;7.00861760722513]
Robot Arm 2 Joint Angles: [0.00208303161433107;-2.19038707965317;0.00208304420217988;0.00706505669424365;0.00208303161433682;2.19123578959542;0.00208303050544444]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.78539602676002;-9.97430990779222;-7.00084677780109]
Robot Arm 1 Joint Angles: [-0.00218788775603952;2.1745482189954;-0.00218788775603313;-0.00740588686305206;-0.00218788775605161;-2.17462402122166;-0.00218788704304091]
Mobile Platform 2 Pose: [-2.35208131043162;9.97525461506675;6.8392106021242]
Robot Arm 2 Joint Angles: [0.00221139775159057;-2.26587780907415;0.00221141033943708;0.00704484046517682;0.00221139775159666;2.26671465060702;0.00221139664270427]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.78539595660333;-9.97259808478388;-6.91581468798367]
Robot Arm 1 Joint Angles: [-0.00225972690260118;2.20060448127733;-0.00225972690259496;-0.00739236789516434;-0.00225972690261343;-2.20063535217658;-0.00225972618960273]
Mobile Platform 2 Pose: [-2.35192083971793;9.97363345169467;6.73006248790452]
Robot Arm 2 Joint Angles: [0.00229412387362444;-2.41242936079838;0.00229413646147118;0.00703191281889227;0.00229407791857386;2.42973170496742;0.00229412276473838]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227853;-9.97409553094106;-6.77330520679708]
Robot Arm 1 Joint Angles: [-0.00236353352892908;2.01516324825293;-0.00236353352892313;-0.00737275824162398;-0.0023635335289416;-1.96106357862124;-0.0023635328159309]
Mobile Platform 2 Pose: [-2.35127993883169;9.97464441272647;6.5742401953748]
Robot Arm 2 Joint Angles: [0.00241804172039455;-2.22698812777398;0.00241805430824147;0.00706346715728875;0.00241799576534414;2.13520739346626;0.00241804084772559]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.97228129699828;-6.73833367577277]
Robot Arm 1 Joint Angles: [-0.0023973975264219;2.09439509667864;-0.00239739752641602;-0.00731935067018818;-0.00239739752643449;-2.09439509417083;-0.00239739681342379]
Mobile Platform 2 Pose: [-2.34892718831815;9.9727595471125;6.52231475726514]
Robot Arm 2 Joint Angles: [0.00245631779109523;-2.22461666560403;0.00245633037894225;0.00696800560749113;0.00245627183604493;2.19713974081322;0.00245631691842638]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540074;-9.97333331315079;-6.69737167009219]
Robot Arm 1 Joint Angles: [-0.00242465195277759;1.92606732656943;-0.00242465195277178;-0.00731416496329334;-0.00242465195279026;-1.9260776954754;-0.00242465123977956]
Mobile Platform 2 Pose: [-2.34888952292109;9.97440151645188;6.48258230747298]
Robot Arm 2 Joint Angles: [0.00248965275840245;-2.03917543257963;0.00248966534624954;0.00693894826192076;0.00248960680335222;1.93445365349305;0.00248965188573367]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395781195851;-9.97294909644746;-6.68023583759776]
Robot Arm 1 Joint Angles: [-0.00243934043716969;1.98753916361096;-0.00243934043716392;-0.00731132947224667;-0.0024393404371824;-1.98755520349902;-0.0024393397241717]
Mobile Platform 2 Pose: [-2.34888950728663;9.97327991598154;6.45993268322855]
Robot Arm 2 Joint Angles: [0.00250566057661427;-2.0943951023932;0.00250567316446141;0.00689308441298584;0.00250561462156409;2.0943951023932;0.00250565970394553]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395772638942;-9.97200303864331;-6.67347995257358]
Robot Arm 1 Joint Angles: [-0.00244810084900341;2.0943951023932;-0.00244810084899765;-0.00730965080542708;-0.00244810084901613;-2.0943951023932;-0.00244810013600543]
Mobile Platform 2 Pose: [-2.34888949752865;9.97341757725506;6.44752098799047]
Robot Arm 2 Joint Angles: [0.00251565274897061;-2.07236752485932;0.00251566533681778;0.00689131064275128;0.00251560679392046;2.07237107239979;0.00251565187630191]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395767401621;-9.97358488642586;-6.65848980933812]
Robot Arm 1 Joint Angles: [-0.00245346200287458;1.90895386925231;-0.00245346200286885;-0.00730863212378707;-0.00245346200288733;-1.84130047961619;-0.00245346128987663]
Mobile Platform 2 Pose: [-2.34888949184029;9.97410858665296;6.44097841588714]
Robot Arm 2 Joint Angles: [0.00252147763457648;-1.96180498577163;0.00252149022242366;0.00689027521911586;0.00252143167952634;1.96181060415937;0.00252147676190779]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395764311851;-9.9716500757212;-6.66429501601706]
Robot Arm 1 Joint Angles: [-0.00245662406489679;2.0943950978115;-0.00245662406489105;-0.00730248004082667;-0.00245662406490952;-2.09439510583112;-0.00245662335189882]
Mobile Platform 2 Pose: [-2.34888948854745;9.97248675432542;6.43412084906964]
Robot Arm 2 Joint Angles: [0.00252484763532606;-2.09439510673882;0.00252486022317325;0.00688967191057411;0.00252480168027593;2.09439511238912;0.00252484676265737]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785395762440954;-9.97334587567019;-6.65299730614471]
Robot Arm 1 Joint Angles: [-0.00245853800054772;1.90895386479396;-0.002458538000542;-0.00730211760364607;-0.00245853800056048;-1.82306748031366;-0.00245853728754978]
Mobile Platform 2 Pose: [-2.34888948656724;9.9744193002622;6.43461283911049]
Robot Arm 2 Joint Angles: [0.00252687350441342;-1.90895387309201;0.00252688609226061;0.0068893138447947;0.00252682754936329;1.79987080117654;0.00252687263174473]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785395761295571;-9.9725430019079;-6.65580126947701]
Robot Arm 1 Joint Angles: [-0.00245971087207978;2.03741082995867;-0.00245971087207406;-0.0072992808023168;-0.00245971087209253;-1.95153011908103;-0.00245971015908183]
Mobile Platform 2 Pose: [-2.3488894853488;9.97280560729984;6.43025725548762]
Robot Arm 2 Joint Angles: [0.00252811932431343;-2.09439510242412;0.00252813191216063;0.00688555681186137;0.00252807336926331;2.05806543212665;0.00252811845164476]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395760593853;-9.97237811570277;-6.65550571971024]
Robot Arm 1 Joint Angles: [-0.0024604294316762;2.06379090445736;-0.00246042943167048;-0.00729756247915696;-0.00246042943168896;-1.97791363022604;-0.00246042871867826]
Mobile Platform 2 Pose: [-2.34888948461832;9.97382038052769;6.43109021634436]
Robot Arm 2 Joint Angles: [0.002528867339181;-1.93203055866103;0.00252887992702819;0.00688472950483089;0.00252882138413087;1.89570254297762;0.00252886646651232]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395760172427;-9.97341831965915;-6.64975907402211]
Robot Arm 1 Joint Angles: [-0.00246086097205636;1.89735724181491;-0.00246086097205065;-0.00729653285710986;-0.00246086097206912;-1.81148202682769;-0.00246086025905842]
Mobile Platform 2 Pose: [-2.34888948417614;9.97386331984277;6.43034402245349]
Robot Arm 2 Joint Angles: [0.00252932013252812;-1.92515976024841;0.00252933272037532;0.00688422150581831;0.002529274177478;1.88883276056302;0.00252931925985944]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395759909198;-9.97165700587365;-6.65837171221346]
Robot Arm 1 Joint Angles: [-0.00246113051844891;2.17916679493719;-0.00246113051844318;-0.00729588029970588;-0.00246113051846165;-2.09329288506478;-0.00246112980545095]
Mobile Platform 2 Pose: [-2.34888948390365;9.97275401791422;6.42838829287203]
Robot Arm 2 Joint Angles: [0.00252959916748753;-2.10264775557075;0.00252961175533474;0.00688390825931614;0.00252955321243741;2.06632138237837;0.00252959829481886]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395759753156;-9.97359339940187;-6.64819520364775]
Robot Arm 1 Joint Angles: [-0.00246129030556191;1.869343452737;-0.0024612903055562;-0.00729550261551014;-0.00246129030557468;-1.78347029823298;-0.00246128959256398]
Mobile Platform 2 Pose: [-2.34888948374224;9.97448280346301;6.43086664825836]
Robot Arm 2 Joint Angles: [0.00252976445095351;-1.82604188510198;0.00252977703880071;0.00688372559752351;0.00252971849590339;1.78971587723318;0.00252976357828484]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785395759655636;-9.97198118931604;-6.65650177558254]
Robot Arm 1 Joint Angles: [-0.00246139016627963;2.12729682409647;-0.0024613901662739;-0.0072952602415042;-0.00246139016629238;-2.04142415434046;-0.00246138945328168]
Mobile Platform 2 Pose: [-2.34888948364148;9.97257109796029;6.42762116112971]
Robot Arm 2 Joint Angles: [0.00252986762166897;-2.13191464863269;0.00252988020951617;0.00688360869297008;0.00252982166661885;2.095588874573;0.0025298667490003]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395759597111;-9.97295374746973;-6.65125334903681]
Robot Arm 1 Joint Angles: [-0.00246145009563331;1.97168737727154;-0.00246145009562759;-0.00729511800759162;-0.00246145009564607;-1.88581499198336;-0.00246144938263537]
Mobile Platform 2 Pose: [-2.34888948358167;9.974120315577;6.43013930094292]
Robot Arm 2 Joint Angles: [0.00252992887479656;-1.88403976243151;0.00252994146264376;0.00688354116478999;0.00252988291974644;1.84771412342818;0.00252992800212789]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785395759561427;-9.97296922404441;-6.6513492560343]
Robot Arm 1 Joint Angles: [-0.00246148663562581;1.96921103745212;-0.0024614866356201;-0.00729503013686692;-0.00246148663563858;-1.88333882790539;-0.00246148592262787]
Mobile Platform 2 Pose: [-2.34888948354501;9.97350506460384;6.42887619334574]
Robot Arm 2 Joint Angles: [0.00252996641344237;-1.98247987583543;0.00252997900128957;0.00688349886322269;0.00252992045839225;1.94615432143524;0.0025299655407737]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785395759539366;-9.97196650325642;-6.6562907865685]
Robot Arm 1 Joint Angles: [-0.00246150922616748;2.1296463091937;-0.00246150922616176;-0.00729497579976558;-0.00246150922618023;-2.04377420832117;-0.00246150851316953]
Mobile Platform 2 Pose: [-2.34888948352263;9.9730599672648;6.42834165383309]
Robot Arm 2 Joint Angles: [0.00252998932703144;-2.05369542449366;0.00253000191487864;0.00688347327513161;0.00252994337198132;2.01736992126964;0.00252998845436277]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.78539575952626;-9.97359580921727;-6.64798611087732]
Robot Arm 1 Joint Angles: [-0.00246152264733956;1.86895732361144;-0.00246152264733385;-0.0072949439527453;-0.00246152264735233;-1.78308528643295;-0.00246152193434162]
Mobile Platform 2 Pose: [-2.34888948350932;9.97438151628494;6.43029979011185]
Robot Arm 2 Joint Angles: [0.00253000295341287;-1.84224756614886;0.00253001554126007;0.00688345815350943;0.00252995699836275;1.8059220931681;0.0025300020807442]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785395759518025;-9.97165958807069;-6.65794808034011]
Robot Arm 1 Joint Angles: [-0.00246153108011986;2.178752686596;-0.00246153108011414;-0.00729492348487617;-0.00246153108013261;-2.09288069035325;-0.00246153036712191]
Mobile Platform 2 Pose: [-2.34888948350101;9.97248064862347;6.42728586679583]
Robot Arm 2 Joint Angles: [0.00253001146467304;-2.1463863823551;0.00253002405252024;0.0068834485243176;0.00252996550962292;2.11006092863271;0.00253001059200437]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785395759513141;-9.97340627578782;-6.64887127655015]
Robot Arm 1 Joint Angles: [-0.00246153608088411;1.89928264003497;-0.0024615360808784;-0.00729491166461823;-0.00246153608089688;-1.81341066743274;-0.00246153536788618]
Mobile Platform 2 Pose: [-2.3488894834961;9.97434465930549;6.43032058642184]
Robot Arm 2 Joint Angles: [0.00253001649080955;-1.8481446676984;0.00253002907865675;0.00688344298987944;0.00252997053575943;1.81181922504489;0.00253001561814088]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785395759510112;-9.97238834370791;-6.65431341078414]
Robot Arm 1 Joint Angles: [-0.00246153918221748;2.06215176531351;-0.00246153918221176;-0.00729490415802862;-0.00246153918223023;-1.97627980772445;-0.00246153846921953]
Mobile Platform 2 Pose: [-2.34888948349307;9.97313790683835;6.42818543652668]
Robot Arm 2 Joint Angles: [0.00253001960072695;-2.0412250589245;0.00253003218857415;0.00688343947346356;0.00252997364567683;2.00489962330383;0.00253001872805828]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785395759508273;-9.97251220481971;-6.6534707095392]
Robot Arm 1 Joint Angles: [-0.00246154106582999;2.04233398293087;-0.00246154106582428;-0.00729489966353731;-0.00246154106584275;-1.9564620343308;-0.00246154035283205]
Mobile Platform 2 Pose: [-2.34888948349123;9.97342190549959;6.42883869938331]
Robot Arm 2 Joint Angles: [0.00253002147852678;-1.99578527103981;0.00253003406637398;0.00688343738735938;0.00252997552347666;1.95945983959135;0.00253002060585811]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785395759507166;-9.97332888203844;-6.64945225394588]
Robot Arm 1 Joint Angles: [-0.00246154219933972;1.91166562522433;-0.00246154219933401;-0.00729489695433561;-0.00246154219935249;-1.82579368204266;-0.00246154148634179]
Mobile Platform 2 Pose: [-2.34888948349013;9.97417786302796;6.42985527025487]
Robot Arm 2 Joint Angles: [0.00253002260477224;-1.87483206524365;0.00253003519261944;0.00688343613115848;0.00252997664972212;1.83850663630758;0.00253002173210356]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785395759506474;-9.97169930686996;-6.65771324223667]
Robot Arm 1 Joint Angles: [-0.00246154290748153;2.1723976504694;-0.00246154290747581;-0.00729489524224964;-0.00246154290749428;-2.08652571071191;-0.00246154219448358]
Mobile Platform 2 Pose: [-2.34888948348945;9.97253778470443;6.42735018003712]
Robot Arm 2 Joint Angles: [0.00253002330516969;-2.13724459621882;0.00253003589301689;0.00688343534121336;0.00252997735011957;2.10091916886264;0.00253002243250102]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785395759506065;-9.97361778204734;-6.64785910460435]
Robot Arm 1 Joint Angles: [-0.00246154332633321;1.86544162109776;-0.0024615433263275;-0.00729489425172447;-0.00246154332634598;-1.77956968332132;-0.00246154261333528]
Mobile Platform 2 Pose: [-2.34888948348905;9.97446869137336;6.43043382991155]
Robot Arm 2 Joint Angles: [0.00253002371823945;-1.82829952873465;0.00253003630608664;0.00688343488603013;0.00252997776318932;1.79197410228884;0.00253002284557077]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785395759505809;-9.97187540409097;-6.656938911363]
Robot Arm 1 Joint Angles: [-0.00246154358894332;2.14422209348063;-0.00246154358893759;-0.00729489361411645;-0.00246154358895607;-2.05835015697939;-0.00246154287594537]
Mobile Platform 2 Pose: [-2.34888948348879;9.97281362666639;6.42764516178973]
Robot Arm 2 Joint Angles: [0.00253002397555501;-2.09310988155872;0.00253003656340221;0.00688343459452808;0.00252997802050489;2.05678445569592;0.00253002310288634]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785395759505656;-9.97307917302321;-6.65059553927168]
Robot Arm 1 Joint Angles: [-0.00246154374588661;1.95161906394996;-0.00246154374588089;-0.00729489324214186;-0.00246154374589937;-1.86574712819268;-0.00246154303288867]
Mobile Platform 2 Pose: [-2.34888948348864;9.97378421577368;6.42936793906919]
Robot Arm 2 Joint Angles: [0.00253002412940975;-1.93781562422177;0.00253003671725695;0.00688343442436452;0.00252997817435963;1.90149019869929;0.00253002325674108]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785395759505562;-9.97283566172529;-6.65207568829744]
Robot Arm 1 Joint Angles: [-0.00246154384200664;1.99058087138579;-0.00246154384200093;-0.00729489301058375;-0.0024615438420194;-1.90470893609162;-0.0024615431290087]
Mobile Platform 2 Pose: [-2.34888948348855;9.97388538854448;6.42930471168863]
Robot Arm 2 Joint Angles: [0.00253002422265249;-1.92162798078912;0.00253003681049969;0.00688343431981533;0.00252997826760237;1.88530255547574;0.00253002334998382]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036872e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.631577e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785395759505504;-9.97208304294275;-6.65574885789258]
Robot Arm 1 Joint Angles: [-0.00246154390119389;2.11099987644941;-0.00246154390118817;-0.00729489286848789;-0.00246154390120664;-2.02512794143944;-0.00246154318819594]
Mobile Platform 2 Pose: [-2.3488894834885;9.97273254009035;6.42763872053371]
Robot Arm 2 Joint Angles: [0.00253002428018759;-2.1060837333862;0.0025300368680348;0.00688343425520726;0.00252997832513748;2.06975830820203;0.00253002340751892]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.811171e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.895041e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.78539575950547;-9.97355589919412;-6.64829011432792]
Robot Arm 1 Joint Angles: [-0.00246154393643776;1.87534287614746;-0.00246154393643205;-0.00729489278473175;-0.00246154393645053;-1.78947094130499;-0.00246154322343983]
Mobile Platform 2 Pose: [-2.34888948348846;9.97448503163168;6.43037160928069]
Robot Arm 2 Joint Angles: [0.00253002431419329;-1.82568508673579;0.00253003690204049;0.00688343421763793;0.00252997835914317;1.78935966162676;0.00253002344152462]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563931e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.238572e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785395759505448;-9.97163424244322;-6.65815397495836]
Robot Arm 1 Joint Angles: [-0.0024615439585739;2.18280795623652;-0.00246154395856817;-0.00729489273103497;-0.00246154395858665;-2.09693602150146;-0.00246154324557594]
Mobile Platform 2 Pose: [-2.34888948348844;9.97258280007734;6.42726970987694]
Robot Arm 2 Joint Angles: [0.00253002433544807;-2.1300421354069;0.00253003692329528;0.00688343419355316;0.00252997838039796;2.09371671034605;0.00253002346277941]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056079e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.403933e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785395759505435;-9.97348102378628;-6.64859180451582]
Robot Arm 1 Joint Angles: [-0.00246154397168094;1.88732294131718;-0.00246154397167523;-0.0072948927000638;-0.00246154397169371;-1.80145100664405;-0.00246154325868301]
Mobile Platform 2 Pose: [-2.34888948348843;9.97409845417643;6.42981648801785]
Robot Arm 2 Joint Angles: [0.00253002434806063;-1.88753747953695;0.00253003693590783;0.00688343417964656;0.00252997839301051;1.8512120545039;0.00253002347539196]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231848e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.614155e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785395759505427;-9.97225080614539;-6.65511445526811]
Robot Arm 1 Joint Angles: [-0.00246154397983688;2.08415776383889;-0.00246154397983116;-0.00729489268030123;-0.00246154397984964;-1.99828582920529;-0.00246154326683894]
Mobile Platform 2 Pose: [-2.34888948348842;9.97353297345678;6.428681087472]
Robot Arm 2 Joint Angles: [0.00253002435578671;-1.97801439467254;0.00253003694363391;0.00688343417094307;0.00252997840073659;1.94168896965691;0.00253002348311804]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531990e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.459657e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785395759505423;-9.97265434030515;-6.65284295629283]
Robot Arm 1 Joint Angles: [-0.00246154398476928;2.01959229826642;-0.00246154398476357;-0.00729489266855363;-0.00246154398478204;-1.93372036365632;-0.00246154327177134]
Mobile Platform 2 Pose: [-2.34888948348842;9.97303365508695;6.42808862453198]
Robot Arm 2 Joint Angles: [0.0025300243605062;-2.05790533384119;0.00253003694835341;0.00688343416567059;0.00252997840545608;2.0215799088361;0.00253002348783753]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692260e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.667472e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.78539575950542;-9.97322813797382;-6.65009145689996]
Robot Arm 1 Joint Angles: [-0.00246154398774938;1.92778467127224;-0.00246154398774367;-0.00729489266141704;-0.00246154398776215;-1.84191273667641;-0.00246154327475145]
Mobile Platform 2 Pose: [-2.34888948348842;9.97439290733038;6.43012915943137]
Robot Arm 2 Joint Angles: [0.00253002436331101;-1.84042497488796;0.00253003695115821;0.00688343416255921;0.00252997840826089;1.8040995498891;0.00253002349064234]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663859e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.143187e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.785395759505418;-9.97176377613675;-6.65747813759201]
Robot Arm 1 Joint Angles: [-0.00246154398960598;2.16208256519771;-0.00246154398960025;-0.00729489265693481;-0.00246154398961873;-2.07621063061084;-0.00246154327660803]
Mobile Platform 2 Pose: [-2.34888948348841;9.97248230782362;6.42710402210439]
Robot Arm 2 Joint Angles: [0.00253002436506346;-2.14612089596869;0.00253003695291067;0.00688343416057615;0.00252997841001334;2.10979547097378;0.00253002349239479]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984807e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.308329e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785395759505417;-9.97362810088841;-6.64792805207277]
Robot Arm 1 Joint Angles: [-0.00246154399070493;1.86379060493077;-0.00246154399069922;-0.0072948926543339;-0.0024615439907177;-1.7779186703491;-0.002461543277707]
Mobile Platform 2 Pose: [-2.34888948348841;9.97433080162542;6.430125054425]
Robot Arm 2 Joint Angles: [0.00253002436609847;-1.85036188767889;0.00253003695394566;0.00688343415943653;0.00252997841104834;1.81403646268627;0.00253002349342979]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059253e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.006098e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785395759505416;-9.97178688319742;-6.65748565585436]
Robot Arm 1 Joint Angles: [-0.0024615439913949;2.15838543548645;-0.00246154399138918;-0.00729489265265837;-0.00246154399140765;-2.07251350090813;-0.00246154327839695]
Mobile Platform 2 Pose: [-2.34888948348841;9.97316516195633;6.42805634685635]
Robot Arm 2 Joint Angles: [0.00253002436673851;-2.03686423473211;0.00253003695458571;0.00688343415871293;0.00252997841168839;2.00053880974093;0.00253002349406984]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857436e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.105687e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785395759505416;-9.97319545975326;-6.65010797578431]
Robot Arm 1 Joint Angles: [-0.00246154399180599;1.93301318655206;-0.00246154399180028;-0.00729489265168518;-0.00246154399181875;-1.84714125197569;-0.00246154327880805]
Mobile Platform 2 Pose: [-2.34888948348841;9.97339348358162;6.42862439191579]
Robot Arm 2 Joint Angles: [0.00253002436712527;-2.0003327746865;0.00253003695497247;0.00688343415828311;0.00252997841207515;1.96400734969618;0.0025300234944566]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905819e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.953774e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785395759505415;-9.97269674450398;-6.65290069318509]
Robot Arm 1 Joint Angles: [-0.00246154399205884;2.01280762643513;-0.00246154399205312;-0.00729489265107502;-0.0024615439920716;-1.92693569185998;-0.0024615432790609]
Mobile Platform 2 Pose: [-2.34888948348841;9.97419686144722;6.42972176624097]
Robot Arm 2 Joint Angles: [0.00253002436735707;-1.87179231618876;0.00253003695520427;0.00688343415802471;0.00252997841230694;1.83546689119896;0.00253002349468839]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634522e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.051716e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785395759505415;-9.97221078172521;-6.65519569974022]
Robot Arm 1 Joint Angles: [-0.00246154399221388;2.09056167103909;-0.00246154399220816;-0.00729489265070351;-0.00246154399222663;-2.00468973646468;-0.00246154327921593]
Mobile Platform 2 Pose: [-2.34888948348841;9.97252829015691;6.42716642469315]
Robot Arm 2 Joint Angles: [0.0025300243675013;-2.13876372263858;0.0025300369553485;0.00688343415786192;0.00252997841245118;2.10243829764911;0.00253002349483263]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.067005e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.644439e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735006134901;-6.64870124197991]
Robot Arm 1 Joint Angles: [-0.00246154399230644;1.8841885886564;-0.00246154399230073;-0.0072948926504832;-0.00246154399231921;-1.79831665408243;-0.00246154327930851]
Mobile Platform 2 Pose: [-2.34888948348841;9.97446296667044;6.43026163966333]
Robot Arm 2 Joint Angles: [0.00253002436758634;-1.82921548047442;0.00253003695543354;0.0068834341577683;0.00252997841253622;1.79289005548513;0.00253002349491767]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.069100e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.427612e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163094321734;-6.65826420090662]
Robot Arm 1 Joint Angles: [-0.00246154399236456;2.18333583229786;-0.00246154399235883;-0.00729489265034229;-0.00246154399237731;-2.09746389772418;-0.0024615432793666]
Mobile Platform 2 Pose: [-2.34888948348841;9.97283574580279;6.42751233993829]
Robot Arm 2 Joint Angles: [0.00253002436763931;-2.08957081929719;0.00253003695548652;0.00688343415770822;0.00252997841258919;2.05324539430803;0.00253002349497064]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.776572e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.006699e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785395759505415;-9.97354092061521;-6.64840078543374]
Robot Arm 1 Joint Angles: [-0.00246154399239889;1.87773944863744;-0.00246154399239318;-0.00729489265026122;-0.00246154399241166;-1.79186751406391;-0.00246154327940096]
Mobile Platform 2 Pose: [-2.34888948348841;9.97375764213797;6.42916050495335]
Robot Arm 2 Joint Angles: [0.002530024367671;-1.94206740566839;0.0025300369555182;0.00688343415767321;0.00252997841262087;1.90574198067929;0.00253002349500232]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247228e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.187460e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.785395759505415;-9.97212038216775;-6.6558824117393]
Robot Arm 1 Joint Angles: [-0.00246154399242036;2.10502560023202;-0.00246154399241463;-0.00729489265020912;-0.00246154399243311;-2.0191536656586;-0.00246154327942241]
Mobile Platform 2 Pose: [-2.34888948348841;9.97391038611463;6.42918158558291]
Robot Arm 2 Joint Angles: [0.00253002436769018;-1.91762836940405;0.00253003695553738;0.00688343415765171;0.00252997841264006;1.88130294441499;0.00253002349502151]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064539e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.858563e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785395759505415;-9.97279446002223;-6.65223019561338]
Robot Arm 1 Joint Angles: [-0.00246154399243326;1.99717314351447;-0.00246154399242755;-0.00729489265017845;-0.00246154399244602;-1.91130120894111;-0.00246154327943532]
Mobile Platform 2 Pose: [-2.34888948348841;9.97271336018161;6.42744152322882]
Robot Arm 2 Joint Angles: [0.00253002436770204;-2.10915251868651;0.00253003695554924;0.00688343415763834;0.00252997841265192;2.07282709369748;0.00253002349503337]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.742991e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.543684e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.785395759505415;-9.97311524782296;-6.65079308472694]
Robot Arm 1 Joint Angles: [-0.00246154399244109;1.94584709539847;-0.00246154399243538;-0.00729489265015967;-0.00246154399245386;-1.85997516082515;-0.00246154327944316]
Mobile Platform 2 Pose: [-2.34888948348841;9.97448764658819;6.43021434289296]
Robot Arm 2 Joint Angles: [0.00253002436770903;-1.82526669363475;0.00253003695555623;0.0068834341576307;0.00252997841265891;1.78894126864574;0.00253002349504036]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155980e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.322852e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785395759505415;-9.97184705328127;-6.6571492964578]
Robot Arm 1 Joint Angles: [-0.00246154399244598;2.14875822206857;-0.00246154399244025;-0.00729489265014786;-0.00246154399245873;-2.06288628749528;-0.00246154327944803]
Mobile Platform 2 Pose: [-2.34888948348841;9.97259623610758;6.42712302553332]
Robot Arm 2 Joint Angles: [0.00253002436771342;-2.12789237053136;0.00253003695556062;0.00688343415762565;0.0025299784126633;2.09156694554236;0.00253002349504475]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302894e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.121812e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362257766344;-6.64808091524349]
Robot Arm 1 Joint Angles: [-0.00246154399244884;1.86467432092128;-0.00246154399244313;-0.00729489265014114;-0.00246154399246161;-1.778802386348;-0.0024615432794509]
Mobile Platform 2 Pose: [-2.34888948348841;9.97407696634186;6.42961806833548]
Robot Arm 2 Joint Angles: [0.00253002436771601;-1.89097553304663;0.00253003695556321;0.00688343415762285;0.00252997841266588;1.85465010805763;0.00253002349504733]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531498e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.573681e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.785395759505415;-9.97171662409789;-6.6579385588234]
Robot Arm 1 Joint Angles: [-0.00246154399245068;2.16962689140944;-0.00246154399244495;-0.00729489265013662;-0.00246154399246343;-2.08375495683617;-0.00246154327945273]
Mobile Platform 2 Pose: [-2.34888948348841;9.97356117099561;6.42856147923235]
Robot Arm 2 Joint Angles: [0.0025300243677176;-1.97350278844729;0.0025300369555648;0.00688343415762104;0.00252997841266748;1.9371773634583;0.00253002349504893]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.613167e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.983936e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785395759505415;-9.97330001676718;-6.64968226679352]
Robot Arm 1 Joint Angles: [-0.00246154399245173;1.91628406432255;-0.00246154399244602;-0.00729489265013417;-0.0024615439924645;-1.83041212974928;-0.0024615432794538]
Mobile Platform 2 Pose: [-2.34888948348841;9.97300785068596;6.42788164219914]
Robot Arm 2 Joint Angles: [0.00253002436771857;-2.06203403799017;0.00253003695556578;0.00688343415761992;0.00252997841266846;2.02570861300118;0.0025300234950499]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408568e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.550711e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785395759505415;-9.97255494590405;-6.6537371298561]
Robot Arm 1 Joint Angles: [-0.00246154399245241;2.03549540242296;-0.00246154399244669;-0.00729489265013252;-0.00246154399246517;-1.9496234678497;-0.00246154327945446]
Mobile Platform 2 Pose: [-2.34888948348841;9.9744037507228;6.4299854288642]
Robot Arm 2 Joint Angles: [0.00253002436771914;-1.83869003209568;0.00253003695556634;0.00688343415761935;0.00252997841266902;1.80236460710669;0.00253002349505047]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394164e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.439664e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785395759505415;-9.97234650754405;-6.65460270477929]
Robot Arm 1 Joint Angles: [-0.00246154399245282;2.06884554002401;-0.0024615439924471;-0.00729489265013153;-0.00246154399246557;-1.98297360545075;-0.00246154327945487]
Mobile Platform 2 Pose: [-2.34888948348841;9.97248490111645;6.42694025727605]
Robot Arm 2 Joint Angles: [0.00253002436771952;-2.14570596911261;0.00253003695556672;0.00688343415761885;0.0025299784126694;2.10938054412363;0.00253002349505085]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956357e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.598879e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785395759505415;-9.97343026926908;-6.64918949930142]
Robot Arm 1 Joint Angles: [-0.00246154399245304;1.89544366401808;-0.00246154399244733;-0.00729489265013102;-0.00246154399246581;-1.80957172944482;-0.00246154327945511]
Mobile Platform 2 Pose: [-2.34888948348841;9.97431640775112;6.42993881506774]
Robot Arm 2 Joint Angles: [0.00253002436771972;-1.85266490756528;0.00253003695556692;0.00688343415761871;0.00252997841266959;1.8163394825763;0.00253002349505104]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260469e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.557216e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164968951921;-6.65826235962286]
Robot Arm 1 Joint Angles: [-0.00246154399245322;2.18033642399812;-0.0024615439924475;-0.00729489265013054;-0.00246154399246597;-2.09446448942486;-0.00246154327945527]
Mobile Platform 2 Pose: [-2.34888948348841;9.97319269455733;6.42793396760458]
Robot Arm 2 Joint Angles: [0.00253002436771986;-2.03245901857146;0.00253003695556706;0.0068834341576185;0.00252997841266974;1.99613359358247;0.00253002349505119]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807923e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.914437e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785395759505415;-9.97358545598195;-6.64829065134618]
Robot Arm 1 Joint Angles: [-0.00246154399245328;1.87061378995892;-0.00246154399244758;-0.00729489265013045;-0.00246154399246605;-1.78474185538566;-0.00246154327945535]
Mobile Platform 2 Pose: [-2.34888948348841;9.97336508692505;6.42841382559521]
Robot Arm 2 Joint Angles: [0.00253002436771993;-2.00487623973607;0.00253003695556714;0.00688343415761842;0.00252997841266981;1.96855081474709;0.00253002349505126]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620323e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.224052e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.785395759505415;-9.97200013539397;-6.65659579684792]
Robot Arm 1 Joint Angles: [-0.00246154399245336;2.12426508403601;-0.00246154399244764;-0.00729489265013021;-0.00246154399246612;-2.03839314946275;-0.00246154327945542]
Mobile Platform 2 Pose: [-2.34888948348841;9.97421533880695;6.4295897849696]
Robot Arm 2 Joint Angles: [0.00253002436771998;-1.86883593863187;0.00253003695556718;0.00688343415761841;0.00252997841266985;1.83251051364288;0.0025300234950513]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640783e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.522354e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785395759505415;-9.97292968713781;-6.65164343752433]
Robot Arm 1 Joint Angles: [-0.00246154399245338;1.9755368050224;-0.00246154399244767;-0.00729489265013019;-0.00246154399246615;-1.88966487044914;-0.00246154327945544]
Mobile Platform 2 Pose: [-2.34888948348841;9.97251962145026;6.42698529907136]
Robot Arm 2 Joint Angles: [0.00253002436772002;-2.14015071570229;0.00253003695556722;0.00688343415761829;0.0025299784126699;2.1038252907133;0.00253002349505135]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711110e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.508828e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785395759505415;-9.97299183541534;-6.65154595274172]
Robot Arm 1 Joint Angles: [-0.0024615439924534;1.96559308061636;-0.00246154399244769;-0.00729489265013015;-0.00246154399246617;-1.8797211460431;-0.00246154327945546]
Mobile Platform 2 Pose: [-2.34888948348841;9.97445660902934;6.430089208466]
Robot Arm 2 Joint Angles: [0.00253002436772002;-1.83023270304965;0.00253003695556722;0.00688343415761837;0.0025299784126699;1.79390727806067;0.00253002349505135]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373557e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.977256e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785395759505415;-9.97194709741018;-6.65673575840169]
Robot Arm 1 Joint Angles: [-0.00246154399245343;2.13275116144264;-0.00246154399244771;-0.00729489265013005;-0.00246154399246618;-2.04687922686938;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.34888948348841;9.9728584153521;6.42738096619453]
Robot Arm 2 Joint Angles: [0.00253002436772004;-2.08594369140789;0.00253003695556725;0.00688343415761828;0.00252997841266993;2.04961826641891;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767804e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.162267e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360123983821;-6.64831637142007]
Robot Arm 1 Joint Angles: [-0.00246154399245341;1.86808837295744;-0.00246154399244771;-0.00729489265013014;-0.00246154399246618;-1.78221643838418;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.34888948348841;9.97373078406725;6.42895288522579]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.94636469698362;0.00253003695556725;0.00688343415761832;0.00252997841266992;1.91003927199464;0.00253002349505137]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002526e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.289181e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166632921239;-6.65828886791101]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17767407308892;-0.00246154399244772;-0.00729489265013;-0.0024615439924662;-2.09180213851566;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34888948348841;9.97393502272408;6.42905819330306]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91368651189195;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.87736108690296;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.187228e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.887951e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785395759505415;-9.97339148554344;-6.64932526492237]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90164906012155;-0.00246154399244771;-0.00729489265013011;-0.00246154399246619;-1.81577712554829;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34888948348841;9.97269482009232;6.4272454011254]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.11211893297327;0.00253003695556726;0.00688343415761826;0.00252997841266994;2.07579350798429;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546799e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.099514e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785395759505415;-9.97241328591609;-6.65456938368534]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.05816100049619;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.97228906592293;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34888948348841;9.97448962367713;6.43005607825373]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82495035940247;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78862493441349;0.00253002349505137]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.136121e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.414945e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724870348897;-6.65398600376929]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.04636116471989;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.96048923014663;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34888948348841;9.97261044309237;6.42697762131482]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.12561925296419;0.00253003695556727;0.00688343415761825;0.00252997841266994;2.0892938279752;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235454e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.318502e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785395759505415;-9.97334546906772;-6.64975107323676]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90901169623616;-0.00246154399244772;-0.00729489265013009;-0.00246154399246619;-1.8231397616629;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34888948348841;9.97405498873265;6.42941883785461]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.89449195052046;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.85816652553148;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.018100e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.865016e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785395759505415;-9.97169001677896;-6.65815098765241]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17388406243769;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.08801212786443;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34888948348841;9.97358926704759;6.42844186448352]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.96900742013009;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9326819951411;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924401e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.458695e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361436263305;-6.64826276794917]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.86598872578265;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.78011679120939;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34888948348841;9.97298240334162;6.42767519056661]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06610561308504;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.02978018809606;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213735e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.566732e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.785395759505415;-9.97189296008914;-6.6572402648311]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.14141313280842;-0.00246154399244773;-0.00729489265013;-0.0024615439924662;-2.05554119823516;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34888948348841;9.97441398274891;6.42984072925747]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83705290791808;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80072748292909;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673361e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.167139e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.780220111501575;-9.97305746965453;-6.65109569841045]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.95597189978402;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.86921966963569;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34708989187625;9.97248836551044;6.42677785109549]
Robot Arm 2 Joint Angles: [0.00253002250507491;-2.02249414147851;0.00253003509292212;0.00688343602026339;0.0025299765500248;2.09439510263075;0.00253002163240625]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.181799e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.028093e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.780220111501575;-9.97285991725256;-6.65233913555237]
Robot Arm 1 Joint Angles: [-0.00246154585509859;1.98758028410009;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.90082805395175;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34708989187625;9.97429643302308;6.42974585515932]
Robot Arm 2 Joint Angles: [0.0025300225050749;-1.73320333945656;0.0025300350929221;0.00688343602026348;0.00252997655002478;1.8051043006088;0.00253002163240623]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.352446e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.113416e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.774214410289993;-9.97206302236907;-6.65624139452831]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.0943951023932;-0.00246154399244773;-0.00729489265013001;-0.0024615439924662;-2.02833123344619;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34708989187625;9.97341967089568;6.42811582713912]
Robot Arm 2 Joint Angles: [0.00253002250507491;-1.8734852798395;0.00253003509292211;0.00688343602026344;0.00252997655002479;1.94538624099175;0.00253002163240624]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.853145e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.867060e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.774214410289993;-9.97356375106397;-6.64863543465974]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.85427851120923;-0.00246154399244772;-0.0072948926501301;-0.00246154399246619;-1.78821464226223;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34708989187625;9.97304685043981;6.42772884579519]
Robot Arm 2 Joint Angles: [0.00253002250507491;-1.93313655277918;0.00253003509292211;0.00688343602026342;0.00252997655002479;2.00503751393143;0.00253002163240624]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.226594e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.676883e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.774214410289993;-9.97163361847856;-6.65855348187595]
Robot Arm 1 Joint Angles: [-0.00246154399245346;2.16309972487513;-0.00246154399244773;-0.00729489265012998;-0.0024615439924662;-2.09703585592812;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34708989187625;9.97447275886377;6.42988095375663]
Robot Arm 2 Joint Angles: [0.0025300225050749;-1.70499120494597;0.0025300350929221;0.00688343602026349;0.00252997655002478;1.77689216609821;0.00253002163240623]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.111571e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.550802e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.774214410289993;-9.97344030356764;-6.64918730418338]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.87403011062256;-0.00246154399244772;-0.0072948926501301;-0.00246154399246619;-1.80796624167555;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34708989187625;9.97253493601396;6.42676947541186]
Robot Arm 2 Joint Angles: [0.00253002250507491;-2.01504286091613;0.00253003509292212;0.00688343602026339;0.0025299765500248;2.08694382206838;0.00253002163240625]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.041296e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.605011e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.774214410289993;-9.97233227648874;-6.65509116188603]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.05131444324687;-0.00246154399244772;-0.00729489265013003;-0.0024615439924662;-1.98525057429986;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97413887847319;6.42944739915825]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82960162796495;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.83031303050265;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.707998e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.473887e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.774214410289993;-9.97253405647388;-6.65385237074533]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.01902964562476;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.95296577667775;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97365129097583;6.42842929107183]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.90761562754233;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.90832703008003;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.579465e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.184613e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.774214410289993;-9.97331756483315;-6.65000850258916]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.89366830814151;-0.00246154399244772;-0.00729489265013009;-0.0024615439924662;-1.8276044391945;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97297572655293;6.4275549997805]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01570593520706;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.01641733774476;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.463841e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.422383e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.774214410289993;-9.97168790276061;-6.6582711288092]
Robot Arm 1 Joint Angles: [-0.00246154399245346;2.15441423974701;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.08835037080001;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745086312402;6.42989361070823]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77044118524302;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.77115258778072;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.704931e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.670096e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.770726236505364;-9.97361102787496;-6.64839024473048]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96897300631141;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79382605948165;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97257121456907;6.42677830725201]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08042785262379;0.00253003695556727;0.00688343415761824;0.00252997841266995;2.08113925516149;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.533757e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.987912e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.770726236505364;-9.97189627101507;-6.65733355510193]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24333410389288;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.06818715706312;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97422849388359;6.42953465790193]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8152631623019;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.8159745648396;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.467501e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.280463e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.770726236505364;-9.9731924770713;-6.65052344746272]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.03594113489573;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.86079418806597;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97352891269423;6.42818145238984]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.92719615259915;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.92790755513684;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.151845e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.924766e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.770726236505364;-9.97266587065082;-6.65345569130186]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.1201981621727;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.94505121534294;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97308467659968;6.42768208568461]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.99827392772765;0.00253003695556726;0.00688343415761827;0.00252997841266994;1.99898533026534;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.936115e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.492567e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.770726236505364;-9.97239488619545;-6.65464312950568]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.1635556750326;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-1.98840872820284;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97446995902949;6.42976357399757]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7766287389579;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.7773401414956;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.417349e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.694242e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.770726236505364;-9.97338140189526;-6.64973600801525]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.00571316306195;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.83056621623219;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254150055781;6.42669325608618]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08518209442586;0.00253003695556727;0.00688343415761824;0.00252997841266995;2.08589349696355;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.067640e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.931795e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.770726236505364;-9.97174401730286;-6.65804027181455]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.2676946978465;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09254775101674;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97430832638898;6.42960510096462]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80248996143828;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.80320136397597;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.094170e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.544707e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363331893278;-6.64835157578481]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96540643705915;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79025949022938;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97340572215729;6.42793487925038]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.94690663850961;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.94761804104731;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.005927e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.691153e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.770726236505364;-9.97176813672991;-6.65802149375881]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.26383558951838;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.08868864268862;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97320015376621;6.42781866762872]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.97979758108268;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.98050898362038;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.434332e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.866192e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.770726236505364;-9.97334851307548;-6.64978092284306]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01097537422734;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.83582842739758;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97441961878812;6.42961492005066]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7846831775764;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.7853945801141;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.437740e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.049212e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.770726236505364;-9.97244704332665;-6.6546308965833]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.15521053404069;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.98006358721093;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252774199436;6.42663324004802]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08738346457902;0.00253003695556727;0.00688343415761824;0.00252997841266995;2.08809486711672;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.818732e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.205828e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.770726236505364;-9.97261323155278;-6.65357387291291]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.12862041785867;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.95347347102891;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9743776724326;6.4296575138693]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79139459446004;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79210599699774;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.631807e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.076762e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.770726236505364;-9.97323280755767;-6.65058082000265]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.02948825707725;-0.00246154585509287;-0.00729489078748494;-0.00246154585511135;-1.85434131024749;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97328368674907;6.42769260562784]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.96643230382449;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.96714370636219;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.645965e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.285749e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.770726236505364;-9.97186152635781;-6.65747825590103]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24889324905358;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.07374630222382;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9733201972838;6.42796166945993]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.96059061826799;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.96130202080568;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.661601e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.436543e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.770726236505364;-9.97361969905483;-6.64850195107513]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96758561753084;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79243867070107;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97435794257896;6.42944856012184]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7945513710425;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.7952627735802;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.064061e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.752921e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.770726236505364;-9.97168670523381;-6.65846937261353]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.2768646288939;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10171768206414;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253018824706;6.4265984464793]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08699206414578;0.00253003695556727;0.00688343415761824;0.00252997841266995;2.08770346668348;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.671939e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.712641e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.770726236505364;-9.97347241087911;-6.64920681475266]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99115172564635;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.81600477881659;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97443598396057;6.42969094895751]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78206474998491;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78277615252261;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.474160e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.710961e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.770726236505364;-9.97223491843642;-6.65576338657514]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.18915051647595;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.01400356964619;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9731648184465;6.42745766748029]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98545123223581;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.9861626347735;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.453257e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.285845e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.770726236505364;-9.97282921397159;-6.65251813861783]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.0940632308494;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.91891628401964;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97344279707889;6.42810792769447]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.94097465105316;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.94168605359086;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.863737e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.137983e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.770726236505364;-9.97305517306883;-6.65156906782262]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.05790977529055;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.88276282846079;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9742853827342;6.42926561266044]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80616094620449;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.80687234874219;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.705084e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.497718e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.770726236505364;-9.97202013521344;-6.65670881705795]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.22351583215398;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.04836888532422;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254879446722;6.42658863550586]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0840150689208;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08472647145849;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.456746e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.394303e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.770726236505364;-9.97357020606575;-6.6488400045102]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.9755044957841;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80035754895433;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97448284784414;6.42970470307772]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77456652861283;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.77527793115053;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.037255e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.668303e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.770726236505364;-9.97165683659528;-6.65865409520671]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28164361105913;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10649666422937;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9730511173745;6.42723302308328]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00364340375521;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.0043548062929;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.306044e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.649792e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.770726236505364;-9.97356179533458;-6.64881422467771]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97685021277194;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80170326594218;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97356594854686;6.42825427232365]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.92127041617857;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92198181871627;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.299778e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.293911e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.770726236505364;-9.97204166198003;-6.6567923049779]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.22007154949859;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.04492460266883;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97420253289087;6.4290674195178]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81941692113615;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.82012832367385;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.799010e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.091774e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.770726236505364;-9.97303201065572;-6.65153149471372]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.06161576138906;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.8864688145593;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97258322317326;6.42660314321089]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07850647595461;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.0792178784923;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.542475e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.758228e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.770726236505364;-9.97285462911132;-6.65266724408647]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.08999680849372;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.91484986166396;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451796502528;6.42969829316874]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76894777963088;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.76965918216858;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.320573e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.278073e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.770726236505364;-9.9722103325942;-6.6557802207534]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.19308425123263;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.01793730440287;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97294451862471;6.42702148081219]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0206992037212;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.0214106062589;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.400019e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.708921e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.770726236505364;-9.97348510521253;-6.64936250016058]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98912063230033;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.81397368547057;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97368770882176;6.42839761119014]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.9017887721939;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9025001747316;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.103329e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.848647e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.770726236505364;-9.97168030492758;-6.65856788262696]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27788867789171;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10274173106195;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97411014857176;6.42885556048393]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83419841219314;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.83490981473084;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.917845e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.190301e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.770726236505364;-9.97361563639035;-6.64860907555311]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96823564384907;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79308869701931;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97263285564326;6.42664089816634]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07056528075416;0.00253003695556727;0.00688343415761825;0.00252997841266994;2.07127668329186;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.911226e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.076342e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.770726236505364;-9.97187871486056;-6.65766105807139]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24614308861422;-0.00246154585509287;-0.00729489078748485;-0.00246154585511135;-2.07099614178446;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745411314277;6.42967143293888]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7652411552442;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76595255778189;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.523672e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.317861e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.770726236505364;-9.97321279596319;-6.65065967676189]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.03269011219451;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.85754316536475;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97284684675166;6.4268256402459]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03632670341049;0.00253003695556726;0.00688343415761826;0.00252997841266994;2.03703810594819;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.152358e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.143963e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.770726236505364;-9.97263972717586;-6.65383026320375]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.12438111816689;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.94923417133713;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97380624838298;6.42853500876919]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88282244239859;0.00253003695556726;0.00688343415761831;0.00252997841266993;1.88353384493629;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.031590e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.884225e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.770726236505364;-9.97242082854798;-6.65474973420228]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.15940489862825;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.98425795179849;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97400916539296;6.42863186470716]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.85035572080212;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.85106712333982;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.476821e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.598353e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.770726236505364;-9.97336531320017;-6.65006232043926]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.0082873542771;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.83314040744734;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97269681044339;6.42670044742033]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06033251273219;0.00253003695556727;0.00688343415761825;0.00252997841266994;2.06104391526988;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.139408e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.876054e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.770726236505364;-9.97175571200992;-6.65821865445129]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26582354471654;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09067659788678;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455222217036;6.42962401293748]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76346663641741;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76417803895511;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.085875e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.648725e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363362194204;-6.64859310769562]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96535795557805;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79021100874829;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97275977947777;6.42664784800365]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05025746723276;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.05096886977045;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.065499e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.921021e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.770726236505364;-9.97175583735423;-6.65832186499103]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26580348962742;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09065654279766;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97391989261815;6.428663751325]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86463936477079;0.00253003695556725;0.00688343415761831;0.00252997841266993;1.86535076730849;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.520761e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.945521e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.770726236505364;-9.97336515036915;-6.64993641251597]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.00831340724086;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.8331664604111;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97390071262752;6.42839841678672]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86770816327134;0.00253003695556725;0.00688343415761831;0.00252997841266993;1.86841956580903;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.551711e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.201978e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.770726236505364;-9.97242108619426;-6.65500345903166]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.15936367522247;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.98421672839271;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97277396545411;6.42677998665688]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04798771101778;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.04869911355548;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.733898e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.556469e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.770726236505364;-9.97263948643323;-6.65367905410606]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.12441963698821;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.94927269015845;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455118040682;6.42955608609951]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76363331858336;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76434472112106;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.722227e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.182847e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.770726236505364;-9.97321301252805;-6.65092560891447]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.03265546181697;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.85750851498721;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726848191787;6.42649016526264]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06225111508233;0.00253003695556727;0.00688343415761825;0.00252997841266994;2.06296251762003;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.766799e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.662662e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.770726236505364;-9.97187855527282;-6.65762970443527]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24616862265404;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.07102167582428;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97402715011993;6.42878139364081]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84747816448686;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.84818956702456;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.676303e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.591941e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.770726236505364;-9.97361570406721;-6.64876604318332]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.9682248155518;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79307786872204;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97378611942711;6.42815755423522]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88604307533837;0.00253003695556726;0.00688343415761831;0.00252997841266993;1.88675447787607;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.091123e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.327391e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.770726236505364;-9.97168039066568;-6.65873912017796]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27787495979663;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10272801296687;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97286298057021;6.42687739053745]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0337452924417;0.00253003695556726;0.00688343415761826;0.00252997841266994;2.03445669497939;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.804693e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.119395e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.770726236505364;-9.97348499631355;-6.649383579953]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98913805613727;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.81399110930751;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745380117642;6.42946786021185]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76574030140304;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76645170394073;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.666868e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.865158e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.770726236505364;-9.97221060552977;-6.6561266090025]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.19304058154205;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.01789363471229;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726232689194;6.426354341363]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07209915657095;0.00253003695556727;0.00688343415761825;0.00252997841266994;2.07281055910865;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.447790e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.175620e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.770726236505364;-9.97285440528925;-6.65262893934796]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.09003262002401;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.91488567319425;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97412672708424;6.42888578637948]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83154585019669;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.83225725273439;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.903470e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.264628e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.770726236505364;-9.97303228398107;-6.6519289130589]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.06157202933323;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.88642508250347;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97366691078199;6.42791185223275]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90511645855719;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.90582786109489;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.888669e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.487082e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.770726236505364;-9.97204147018248;-6.6568385552838]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.22010223710867;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.04495529027891;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97296232043124;6.42699024256759]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01785091467734;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.01856231721504;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.486262e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.200502e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.770726236505364;-9.97356193446087;-6.64912635705769]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97682795256549;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80168100573573;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451278487897;6.42935969776874]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76977660303935;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77048800557705;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.962944e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663316e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.770726236505364;-9.97165687398125;-6.65889152269404]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28163762930462;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10649068247486;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97257620986474;6.42624178824708]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07962860531592;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08034000785362;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.354067e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.175635e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.770726236505364;-9.97357015513943;-6.64901325171173]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97551264399594;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80036569716618;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97421752929617;6.42897508621767]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81701749628805;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.81772889882575;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.578882e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.480314e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.770726236505364;-9.97202041679397;-6.65713911547165]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.22347077927011;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.04832383244035;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97354479070177;6.42766409193242]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.92465567139242;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92536707393012;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.832226e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.240188e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.770726236505364;-9.9730549679836;-6.65165395280774]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.05794258892813;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.88279564209837;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97307027899837;6.42711586756969]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00057754393602;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.00128894647371;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.524880e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.596959e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.770726236505364;-9.97282954855501;-6.65303732185023]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.09400969750247;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.91886275067271;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97447563797913;6.42923212259676]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77572010701471;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.77643150955241;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.609009e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.113585e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.770726236505364;-9.97223469428391;-6.65589473568088]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.18918638087889;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-2.01403943404913;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254447863388;6.42615355335268]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08470560225428;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08541700479198;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.444071e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.492818e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.770726236505364;-9.97347262888177;-6.64967047896745]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99111684522042;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.81596989839066;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97429865436093;6.42904775248143]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80403748592593;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.80474888846363;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.127078e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.988819e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.770726236505364;-9.97168668291428;-6.65877316754781]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27686820002019;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10172125319043;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97342161144706;6.42741721055379]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.94436435214559;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.94507575468328;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.866705e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.431158e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.770726236505364;-9.97361971400704;-6.64883070031816]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96758322517821;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79243627834845;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97318500929143;6.42725137211285]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98222069704641;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.9829320995841;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.320013e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.191435e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.770726236505364;-9.97186180254092;-6.65798523781752]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24884905975718;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.07370211292742;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97442679092185;6.42908583174011]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78353563617898;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78424703871668;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.585213e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.859433e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.770726236505364;-9.9732326271916;-6.65079844840953]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.02951711564775;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.85437016881799;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9725286456738;6.42609029251638]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08723887586737;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08795027840506;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.154815e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.023419e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.770726236505364;-9.97261363023622;-6.65420446873667]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.12855662850867;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.95340968167891;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97436937709873;6.42910253418704]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79272184787901;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79343325041671;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.144806e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.938139e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.770726236505364;-9.97244678677798;-6.65485628625439]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.1552515818282;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.98010463499844;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97329932989206;6.42717423494589]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.96392940094628;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96464080348398;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.541146e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.826023e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.770726236505364;-9.97334882054017;-6.65039064727259]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01092617987729;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.83577923304753;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9733045613152;6.42739369781533]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.96309237324288;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96380377578057;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.220185e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.138399e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.770726236505364;-9.97176804211357;-6.65839384005698]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26385072813325;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.08870378130349;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97436656163886;6.42892171064243]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79317232145824;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79388372399594;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.081527e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.394372e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363341144318;-6.64883733420137]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96539163539532;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79024468856556;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252899773785;6.42605224753929]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08718254561891;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08789394815661;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.130846e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.703558e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.770726236505364;-9.9717442660391;-6.65861851295803]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26765490004858;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09250795321882;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97442913073423;6.4291384507856]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78316126619871;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78387266873641;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.614460e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.951833e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.770726236505364;-9.97338125731934;-6.65009467594937]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.00573629520905;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.83058934837929;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97317995468265;6.42693820421055]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98302943445083;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.98374083698853;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.681120e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.818783e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.770726236505364;-9.97239534604588;-6.65537479697295]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.16348209896274;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.98833515213297;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9734269291312;6.42753968927818]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.94351352268283;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.94422492522053;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.661889e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.211892e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.770726236505364;-9.97266558474014;-6.65378506227974]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.1202439078815;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.94509696105174;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97429538561748;6.42874084969242]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8045604848782;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80527188741589;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.820948e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.914538e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.770726236505364;-9.97319288642167;-6.65127200408542]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.0358756388377;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.86072869200794;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254552806756;6.42603923385275]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0845376928659;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08524909540359;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.898311e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.494902e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.770726236505364;-9.97189609214477;-6.65777860409787]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24336272314031;-0.00246154585509287;-0.00729489078748485;-0.00246154585511135;-2.06821577631055;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97447748608055;6.42915476922174]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77542441078765;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.77613581332535;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.693795e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.627851e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.770726236505364;-9.97361121368245;-6.64903271686401]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96894327711295;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79379633028319;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97306548982941;6.42671208981738]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00134381096881;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.00205521350651;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.121449e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.732520e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.770726236505364;-9.97167484411873;-6.6590049647135]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.2787624073078;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10361546047804;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97355010499256;6.42768617377149]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.92380538486514;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92451678740284;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.949376e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.866021e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.770726236505364;-9.97349703765046;-6.64956320053786]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98721144223044;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.81206449540068;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97421383685312;6.42854456054492]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81760828717606;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.81831968971376;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.876071e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.995590e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.770726236505364;-9.9721866771226;-6.65648777498551]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.19686912668858;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.02172217985882;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97257793662704;6.42605064184602]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07935232334925;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08006372588695;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.448945e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.158067e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.770726236505364;-9.97287929211905;-6.65274132656059]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.08605072725685;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.91090378042708;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451413058992;6.42915097955761]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76956128928735;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77027269182505;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.948427e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.728721e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.770726236505364;-9.97300916791139;-6.65228980540621]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.06527060048268;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.89012365365292;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97295788083899;6.42649872206262]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01856124943659;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.01927265197428;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.088056e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.111666e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.770726236505364;-9.97206315558314;-6.65696654309675]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.21663257300214;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.04148562617238;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97367213572914;6.42783004560744]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.90428046701345;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.90499186955115;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.518552e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.931372e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.770726236505364;-9.9735531889229;-6.64941515176558]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97822723864069;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80308029181093;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9741226485773;6.42833439093411]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8321984113071;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.8329098138448;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.904724e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.408338e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.770726236505364;-9.97165767332538;-6.65912509508965]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28150973424349;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10636278741373;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97262564033509;6.42608545182765]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07171973006183;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07243113259953;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.411111e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.046601e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.770726236505364;-9.97357797586224;-6.64921511465844]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.9742613283457;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79911438151594;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453884895964;6.42912677139597]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76560635013335;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76631775267104;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.879802e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.108691e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.770726236505364;-9.9719997052006;-6.6574831382821]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.22678463420936;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.0516376873796;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97285896809429;6.42630072930693]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03438728858923;0.00253003695556726;0.00688343415761826;0.00252997841266994;2.03509869112693;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.870547e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.407596e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.770726236505364;-9.97307750794141;-6.65177858298037]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.0543361956794;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.87918924884964;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9737911752613;6.42796834609806]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88523414186741;0.00253003695556726;0.00688343415761831;0.00252997841266993;1.88594554440511;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.510591e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.303241e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.770726236505364;-9.97280437601286;-6.6534077424937]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.09803730424706;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.9228903574173;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97402273187866;6.42811213659073]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84818508308962;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.84889648562731;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.925324e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023803e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.770726236505364;-9.97225923019978;-6.6560083934012]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.18526063434063;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-2.01011368751087;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97268779086376;6.42614225907224]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06177564547377;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.06248704801147;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.491626e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.478762e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.770726236505364;-9.97345970979648;-6.64998070440759]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99318389886724;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.81803695203748;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455150684859;6.42908201340035]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76358108790044;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76429249043814;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.160991e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.676911e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.770726236505364;-9.97169377154734;-6.65897486862751]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27573401873092;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10058707190116;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97277044941824;6.42612049202786]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04855027675727;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.04926167929497;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.823192e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.107640e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.770726236505364;-9.97362326685476;-6.64905508612466]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96701476954368;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79186782271392;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97390552767119;6.42809833078412]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86693775628517;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.86764915882287;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.763073e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.884625e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.770726236505364;-9.97184554185105;-6.65830605887828]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.25145077013693;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.07630382330717;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9739151900018;6.42787984818215]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86539178338697;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.86610318592467;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.652413e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.668887e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.770726236505364;-9.97325196699967;-6.65093978282394]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.02642274635801;-0.00246154585509287;-0.00729489078748494;-0.00246154585511135;-1.85127579952825;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97276329639334;6.42621930370171]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04969476074114;0.00253003695556726;0.00688343415761826;0.00252997841266994;2.05040616327883;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.159173e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.954709e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.770726236505364;-9.97258760007405;-6.65457820625757]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.13272145445668;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.95757450762691;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455203906166;6.42901673804139]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76349593380934;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76420733634703;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.276509e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.376000e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.770726236505364;-9.97247273875052;-6.65496289817632]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.15109926622191;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.97595231939215;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97269385073349;6.42596010913833]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06080606631743;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.06151746885513;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.610707e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.636647e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.770726236505364;-9.97333193460141;-6.65072092970533]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01362793007897;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.83848098324921;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97401367728657;6.4282175186511]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84963381782393;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.85034522036162;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.310274e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.355830e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.770726236505364;-9.97178098921018;-6.6585659222795]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26177919267582;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.08663224584606;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9738013256799;6.42763983011584]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88361007489119;0.00253003695556726;0.00688343415761831;0.00252997841266993;1.88432147742889;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.080973e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.926307e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363269341644;-6.649084222252]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96550651967346;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.7903595728437;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97285084430218;6.4263145011049]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03568709532662;0.00253003695556726;0.00688343415761826;0.00252997841266994;2.03639849786432;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.202664e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.014693e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.770726236505364;-9.97173342472183;-6.65891143112328]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26938951081094;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09424256398118;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454044322805;6.42893113312216]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76535126718793;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76606266972563;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.716011e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.169867e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.770726236505364;-9.97339683451577;-6.65025571117248]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.00324394378086;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.8280969969511;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97263050172565;6.42582137115561]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07094190757084;0.00253003695556727;0.00688343415761825;0.00252997841266994;2.07165331010854;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.826292e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.671002e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [0.770726236505364;-9.9723698423047;-6.65574481441185]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.16756269755279;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-1.99241575072303;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97411430482152;6.42832372191033]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83353341223274;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.83424481477044;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.601711e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.477778e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [0.770726236505364;-9.97269150784301;-6.65389199263026]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.11609621142364;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.94094926459388;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97368263858665;6.42739462718422]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90260000981193;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90331141234962;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.928888e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.735382e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [0.770726236505364;-9.97317244298175;-6.65161993209485]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.03914658922538;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.86399964239562;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97294892382318;6.42642547174404]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01999437196671;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.0207057745044;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.041203e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.415890e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [0.770726236505364;-9.97191411534442;-6.65792506449447]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24047901119777;-0.00246154585509288;-0.00729489078748486;-0.00246154585511135;-2.06533206436801;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451677953767;6.42882554068904]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76913745764759;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76984886018529;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.714104e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.036990e-82。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [0.770726236505364;-9.97360623405769;-6.64930193712176]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96974001707398;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79459307024422;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97258151322149;6.42570573476361]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07878006823678;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07949147077447;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.117270e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.023197e-83。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [0.770726236505364;-9.97167006162837;-6.65926692883653]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27952760576625;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10438065893649;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97420629083358;6.42841505816536]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81881565030171;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.8195270528394;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.107226e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.617588e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [0.770726236505364;-9.9735085384722;-6.64974565868751]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98537131075302;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.81022436392326;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97356081029025;6.42714699518324]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.92209253723478;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92280393977248;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.102638e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.926453e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [0.770726236505364;-9.97216314988811;-6.65684680346148]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.20063348420687;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.02548653737711;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97305585019306;6.42654957298868]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00288615278467;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.00359755532237;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.456545e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023847e-84。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [0.770726236505364;-9.97290386071592;-6.65285537109508]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.08211975175835;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.90697280492859;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9744811765575;6.42870046284472]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77483393447543;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77554533701313;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.998891e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.328451e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [0.770726236505364;-9.97298584169747;-6.65265165719203]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.06900279471026;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.8938558478805;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254775455729;6.42561429591844]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0841814545084;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08489285704609;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.304682e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.581544e-85。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [0.770726236505364;-9.97208516799961;-6.65709289904279]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.21311058636627;-0.00246154585509288;-0.00729489078748487;-0.00246154585511135;-2.03796363953651;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97428870905802;6.42848994857224]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80562873439215;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80634013692985;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.553741e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.464372e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [0.770726236505364;-9.97354397632819;-6.64970634923382]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97970125379485;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80455430696509;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97343767530887;6.4268998533902]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.94179413425611;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.94250553679381;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.861749e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.396791e-86。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [0.770726236505364;-9.97165922508259;-6.65935486245336]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28126145308972;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10611450625996;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97316979350209;6.42668393816749]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98465522334046;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.98536662587815;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.477433e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.611387e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [0.770726236505364;-9.9735852627257;-6.64941978644165]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97309543019309;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79794848336333;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97443384260415;6.42855657304988]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78240736701074;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78311876954844;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.204193e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.489314e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [0.770726236505364;-9.97197953954312;-6.65782431469777]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.23001113940544;-0.00246154585509288;-0.00729489078748486;-0.00246154585511135;-2.05486419257568;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252983168878;6.42554776254952]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08704911346994;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08776051600763;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.914152e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.277984e-87。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [0.770726236505364;-9.97309962225647;-6.65190542855665]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.05079790526982;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.87565095844006;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97436081254169;6.42854710593183]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7940921770046;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79480357954229;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.499400e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.473863e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [0.770726236505364;-9.97277913065999;-6.6537784082404]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.10207656070554;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.92692961387578;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97331517901802;6.42665622007843]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.96139354079253;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96210494333022;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.272773e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.069412e-88。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [0.770726236505364;-9.97228391689024;-6.65612131279265]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.18131076386603;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-2.00616381703627;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97328881541839;6.42682552797376]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.9656117167326;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.9663231192703;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.542934e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.816900e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [0.770726236505364;-9.97344635641794;-6.6502931287584]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99532043943417;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82017349260441;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97437508167821;6.42839473098125]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79180911516194;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79252051769964;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.205027e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.862306e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [0.770726236505364;-9.97170155628795;-6.65917306046109]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27448846023258;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09934151340282;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252806893436;6.42550643121202]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08733115417793;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08804255671562;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.786690e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.070605e-89。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [0.770726236505364;-9.97362630077446;-6.64928220165694]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96652934239134;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79138239556158;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97442201525836;6.4285855161058]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78429974233735;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78501114487505;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.024374e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.182141e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [0.770726236505364;-9.97182993977198;-6.65862348958234]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.25394710278769;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.07880015595792;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97319532574855;6.42641913617339]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98057006390741;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.98128146644511;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.725596e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.856376e-90。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [0.770726236505364;-9.97327081228319;-6.65108369706912]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.02340750099423;-0.00246154585509287;-0.00729489078748494;-0.00246154585511135;-1.84826055416447;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97341091509104;6.42697119651327]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.94607576910851;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.9467871716462;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.165401e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.573293e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [0.770726236505364;-9.97256165671139;-6.6549513754355]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.13687239248146;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.9617254456517;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97430531262327;6.42821599898731]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8029721639517;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.8036835664894;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.427663e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.890265e-91。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [0.770726236505364;-9.97249866289618;-6.65506967966032]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.14695140291484;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.97180445608508;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254249798418;6.4254901731792]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08502250620695;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08573390874464;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.685169e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.340791e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [0.770726236505364;-9.97331466632521;-6.65105310776025]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01639085427128;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.84124390744152;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97447187145605;6.42860441543962]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77632275070703;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.77703415324473;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.410032e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.323415e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [0.770726236505364;-9.9717945346027;-6.65873499618911]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.25961192987214;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.08446498304238;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97308012252311;6.42619158522042]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.99900257997789;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.99971398251559;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.084093e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.134072e-92。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363147382791;-6.64933373875911]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.9657016538394;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79055470700964;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97353408255722;6.42711776973861]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.92636897452037;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92708037705807;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.253291e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.704743e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [0.770726236505364;-9.97172331455568;-6.65920061681077]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27100713739591;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09586019056615;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97422508996625;6.42802165850355]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81580778907443;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.81651919161213;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.826063e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.289086e-93。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [0.770726236505364;-9.97341188300918;-6.65041951379449]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.00083618483638;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82568923800662;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97257285677163;6.42549843390728]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08016510021433;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08087650275203;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.987634e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.638257e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [0.770726236505364;-9.97234459393332;-6.65611341781658]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.17160243697357;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-1.99645549014381;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451005464039;6.42860326646492]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77021344121261;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77092484375031;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.552525e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.741574e-94。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [0.770726236505364;-9.97271723784628;-6.65399993667037]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.11197941089902;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.93683246406926;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97297152457533;6.4259764186193]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01637825162164;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.01708965415934;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.047096e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.238741e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [0.770726236505364;-9.97315169620729;-6.65196931841764]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.04246607313866;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.8673191263089;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97365635513769;6.42726212966137]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90680536164447;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.90751676418217;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.196260e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.100432e-95。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [0.770726236505364;-9.97193260314759;-6.65806919568259]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.23752096269069;-0.00246154585509288;-0.00729489078748486;-0.00246154585511135;-2.06237401586093;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97413512476242;6.42781322512216]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83020222168858;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.83091362422628;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.737271e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.062248e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [0.770726236505364;-9.97360077137338;-6.64957366877112]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97061404656361;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79546709973385;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97261859849169;6.42553024634981]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07284642500427;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07355782754196;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.096652e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.618656e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [0.770726236505364;-9.97166603852978;-6.65952503858758]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28017130153937;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10502435470961;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745363378991;6.42858173410372]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76600811981919;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76671952235689;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.279644e-97。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.341023e-96。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [0.770726236505364;-9.97351950262362;-6.64993093514022]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98361704652647;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80847009969671;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97287138763678;6.4257762930136]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03240016178992;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.03311156432761;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.347540e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.801028e-97。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [0.770726236505364;-9.97214003984513;-6.65720361673316]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.20433109108484;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.02918414425508;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9737758707697;6.42740129535343]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88768286052375;0.00253003695556726;0.00688343415761831;0.00252997841266993;1.88839426306145;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.470461e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.691498e-97。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [0.770726236505364;-9.97292809814167;-6.65297113972277]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.07824176363758;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.90309481680782;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.974036303596;6.42759246097624]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84601360831499;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.84672501085269;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.055201e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.562546e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [0.770726236505364;-9.97296232234201;-6.65301438013711]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.07276589158389;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.89761894475413;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97267890859948;6.42558425496275]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06319680775906;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.06390821029676;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.537755e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.920218e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [0.770726236505364;-9.97210748414586;-6.65721774101513]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.20954000296674;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.03439305613698;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455057714051;6.42853966466887]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76372984119413;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76444124373182;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.591973e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.062319e-98。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [0.770726236505364;-9.97353430365747;-6.64999990951898]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98124888110977;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80610193428001;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97278142932897;6.42559362249678]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04679349103941;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.0475048935771;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.833314e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.296229e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [0.770726236505364;-9.97166151912678;-6.65958087767414]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28089440602082;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10574745919105;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9738909125392;6.42753249217632]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86927617740286;0.00253003695556726;0.00688343415761832;0.00252997841266993;1.86998757994055;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.553388e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.422655e-99。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [0.770726236505364;-9.97359202105452;-6.64962723934165]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97201409758156;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.7968671507518;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97392970357555;6.42736138248084]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86306961158725;0.00253003695556726;0.00688343415761832;0.00252997841266993;1.86378101412495;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.553888e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.896350e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [0.770726236505364;-9.97195993139426;-6.65816258988616]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.23314844322381;-0.00246154585509288;-0.00729489078748486;-0.00246154585511135;-2.05800149639405;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97275272623836;6.42565874522902]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05138598553774;0.00253003695556727;0.00688343415761826;0.00252997841266994;2.05209738807544;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.963183e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.917739e-100。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [0.770726236505364;-9.97312130341999;-6.65203452897835]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.04732891910718;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.87218197227741;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455269863476;6.4284770698322]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76339040211421;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7641018046519;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.491132e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.679549e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [0.770726236505364;-9.97275383165675;-6.65414922161698]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.10612440122447;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.93097745439471;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97270319893471;6.42543054362446]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0593103541212;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06002175665889;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.655092e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.337297e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [0.770726236505364;-9.97230873127024;-6.65623361090752]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.17734046306659;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-2.00219351623683;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97399994055229;6.42765320348237]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.85183169530863;0.00253003695556725;0.00688343415761832;0.00252997841266993;1.85254309784633;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.598267e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.546945e-101。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [0.770726236505364;-9.97343257740258;-6.65060770336636]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99752508189178;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82237813506202;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97381660073675;6.42712226041868]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88116606579569;0.00253003695556726;0.00688343415761832;0.00252997841266993;1.88187746833339;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.259207e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.768006e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [0.770726236505364;-9.97171002216171;-6.65936781975943]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27313392043195;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09798697360219;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97283876690288;6.42575167413702]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03761947921501;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.0383308817527;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.756677e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.302262e-102。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [0.770726236505364;-9.97362882161499;-6.64951201543464]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96612600790672;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79097906107696;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454269193456;6.42839411721099]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76499147414562;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76570287668331;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.076574e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.342861e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [0.770726236505364;-9.97181500248155;-6.65893750224661]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.25633706925772;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.08119012242796;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97263805262668;6.42528888804671]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06973376340696;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07044516594465;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.805123e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.709784e-103。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [0.770726236505364;-9.97328916056169;-6.65123020499741]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.02047177643436;-0.00246154585509287;-0.00729489078748494;-0.00246154585511135;-1.8453248296046;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97410160985649;6.42776120287102]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83556460663673;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.83627600917442;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.173481e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.653069e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [0.770726236505364;-9.9725358199212;-6.65532387738902]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.14100627891296;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.9658593320832;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97369846895322;6.42687760842411]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9000671511597;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.9007785536974;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.599468e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.997146e-104。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [0.770726236505364;-9.97252453822976;-6.65517673748167]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.14281134954266;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.9676644027129;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97293554504831;6.42586069997381]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.02213497594625;0.00253003695556726;0.00688343415761827;0.00252997841266994;2.02284637848394;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.765827e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.117164e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [0.770726236505364;-9.97329702690686;-6.65138712030629]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01921316120716;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.8440662143774;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97452060880557;6.42829112831741]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76852477478483;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76923617732253;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.519872e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.470333e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [0.770726236505364;-9.97180865932899;-6.6589011581399]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.25735197366729;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.08220502683753;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97258713078216;6.42517015714429]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07788125853027;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07859266106796;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.090793e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.289874e-105。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [0.770726236505364;-9.97362975863031;-6.64958585059747]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96597608545596;-0.00246154585509287;-0.00729489078748497;-0.00246154585511134;-1.79082913862619;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97419477542723;6.42785456846893]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82065811531852;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.82136951785621;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.273842e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.254176e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [0.770726236505364;-9.97171393592768;-6.65948607111321]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27250771787578;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09736077104602;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97357696665404;6.4266301558963]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91950751902866;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92021892156635;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.945340e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.472472e-106。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [0.770726236505364;-9.97342640428072;-6.65058607715722]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99851278138941;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82336583455965;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97304139786631;6.42598321360781]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00519852506658;0.00253003695556726;0.00688343415761828;0.00252997841266994;2.00590992760428;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.016564e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.568667e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [0.770726236505364;-9.97231961941105;-6.65648051622318]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.17559836053689;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-2.00045141370713;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97448656826109;6.42816858352779]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7739712619012;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77468266443889;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.513621e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.829045e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [0.770726236505364;-9.97274275759564;-6.65410898219588]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.1078962510031;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.93274930417334;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255133537926;6.42507549536762]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08360852299339;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08431992553109;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.176768e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.004080e-107。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [0.770726236505364;-9.97313066034149;-6.65232008748713]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.04583181166666;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.8706848648369;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9742784866655;6.42793168269659]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80726431719606;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80797571973376;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.364360e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.186889e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [0.770726236505364;-9.97195153376495;-6.65821110802573]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.2344920639134;-0.00246154585509288;-0.00729489078748486;-0.00246154585511135;-2.05934511708364;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9734539097309;6.42638280290146]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.93919662673154;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.93990802926924;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.763352e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.126218e-108。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [0.770726236505364;-9.97359483184031;-6.6498478764083]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97156437185566;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.7964174250259;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97315451329675;6.42611637624265]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98710005619561;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.9878114587333;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.078871e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.865669e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [0.770726236505364;-9.97166276948083;-6.65977932331692]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28069434937293;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10554740254317;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97444076724678;6.42802713276183]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78129942419082;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78201082672852;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.468221e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.941393e-109。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [0.770726236505364;-9.97352993418671;-6.65011900931973]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98194799643131;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80680104960155;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253130789256;6.42500566281196]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08681292086655;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08752432340424;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.611857e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.077256e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [0.770726236505364;-9.97211736234684;-6.65755814051502]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.20795949081057;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.03281254398081;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97435197432328;6.42799122147915]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79550629195145;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79621769448914;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.490609e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.886996e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [0.770726236505364;-9.97295199225648;-6.65308869518619]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.07441870526825;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.89927175843848;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97333123106181;6.42613855774501]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.95882521378644;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.95953661632414;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.088602e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.909995e-110。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [0.770726236505364;-9.97293862699105;-6.65337788546435]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.07655714773707;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.90141020090731;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97327296556367;6.42625716895095]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.9681476934885;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96885909602619;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.790690e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.705001e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [0.770726236505364;-9.97213008089528;-6.65734118610781]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.20592452306128;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.03077757623152;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97438349603641;6.42786760998024]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79046281785036;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79117422038806;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.633407e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.866211e-111。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [0.770726236505364;-9.97352417800531;-6.65029579209571]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98286898545627;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80772203862651;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252741035953;6.42496101104667]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08743652615023;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08814792868792;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.818839e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.733072e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [0.770726236505364;-9.97166454477685;-6.65980319634217]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28041030200998;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10526335518022;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97441463258653;6.42803213638507]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78548096983117;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78619237236886;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.639431e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.046292e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [0.770726236505364;-9.97359825625964;-6.6498374450771]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97101646476299;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79586951793323;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9732109296526;6.42590046181299]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.97807343925951;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.9787848417972;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.930923e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.224250e-112。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [0.770726236505364;-9.9719408915579;-6.65849791287755]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.2361948170413;-0.00246154585509288;-0.00729489078748486;-0.00246154585511135;-2.06104787021154;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97339475987039;6.42640245678084]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.94866060441317;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.94937200695086;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.017811e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.901882e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [0.770726236505364;-9.97314254466515;-6.65216591991021]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.04393031988146;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.8687833730517;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9743151570397;6.4276910495218]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80139705732426;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80210845986195;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.485637e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.227044e-113。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [0.770726236505364;-9.97272849812008;-6.65452008559151]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.11017776709247;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.93503082026271;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253971325309;6.42494146769051]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08546806318152;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08617946571922;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.076583e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.841094e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [0.770726236505364;-9.97233365064555;-6.65634540284978]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.17335336301689;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-1.99820641618713;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97446599861976;6.42805363245477]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7772624045144;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7779738070521;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.657930e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.488987e-114。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [0.770726236505364;-9.97341838160619;-6.65092437863386]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99979640931442;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82464946248466;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97309501462634;6.4256715095492]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.99661984346123;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.99733124599893;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.323615e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.432158e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [0.770726236505364;-9.97171915378386;-6.65955922522515]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.2716728608872;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09652591405744;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97351788499296;6.42654906581077]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.92896058480197;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92967198733966;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.732710e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.341240e-115。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363083522473;-6.6497444958582]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96580383034893;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79065688351917;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97423628550082;6.42749870259189]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81401650354479;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.81472790608249;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.133332e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.333875e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [0.770726236505364;-9.97180073536413;-6.65924807312255]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.25861980804516;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.0834728612154;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97256799298295;6.42494653400554]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08094330640455;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08165470894225;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.891401e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.045224e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [0.770726236505364;-9.97330700994838;-6.65137931739819]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01761587456413;-0.00246154585509287;-0.00729489078748494;-0.00246154585511135;-1.84246892773437;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97450573151064;6.4280551440203]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77090514197337;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77161654451107;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.183407e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.112247e-116。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [0.770726236505364;-9.97251010919839;-6.65569561482384]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.14511999456245;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.96997304773268;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97298545021268;6.42545457257603]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01415014964676;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.01486155218445;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.516541e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.718462e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [0.770726236505364;-9.97255034436826;-6.655284175408]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.13868236738333;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.96353542055357;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97364036957923;6.4266938670406]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9093630509984;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.9100744535361;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.853269e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.950569e-117。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [0.770726236505364;-9.97327902779443;-6.65172290507288]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.0220930191967;-0.00246154585509287;-0.00729489078748494;-0.00246154585511135;-1.84694607236694;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97414757042985;6.42729205256879]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82821091490017;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.82892231743787;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.640267e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.835769e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [0.770726236505364;-9.97182334419202;-6.65906450562191]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.25500239558149;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.07985544875173;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97261173965049;6.4249752964929]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07394383959661;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.0746552421343;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.101005e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.030925e-118。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [0.770726236505364;-9.97362755376708;-6.64984052459003]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96632886357235;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79118191674259;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453359233783;6.42803631072854]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76644740962218;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76715881215988;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.297633e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.533594e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [0.770726236505364;-9.97170528847942;-6.65976779878534]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27389130959798;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09874436276822;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97288410697247;6.42525233526534]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03036506808002;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03107647061772;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.074653e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.114535e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [0.770726236505364;-9.97344040021381;-6.65075539247402]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.9962734320951;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82112648526534;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9737603362068;6.42683385829453]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.89016839058738;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.89087979312508;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.036088e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.684619e-119。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [0.770726236505364;-9.97229493671435;-6.65684602132152]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.17954759200949;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-2.00440064517973;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9740498739492;6.42707282782563]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84384235180301;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.8445537543407;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.484404e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.209654e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [0.770726236505364;-9.97276805068211;-6.65421921326011]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.10384935716657;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.92870241033681;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97267017317633;6.42502644975363]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06459447546223;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06530587799992;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.295114e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.906006e-120。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [0.770726236505364;-9.97310934985919;-6.65267216276075]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.04924148883458;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.87409454200482;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745494269515;6.4279969560558]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76391387143459;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76462527397228;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.546478e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.734030e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [0.770726236505364;-9.9719708853703;-6.658350912054]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.23139580705707;-0.00246154585509288;-0.00729489078748486;-0.00246154585511135;-2.05624886022731;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97279272199355;6.42506724504416]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04498666470743;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04569806724513;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.792421e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.129028e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [0.770726236505364;-9.97358842170619;-6.65012452438579]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97258999331437;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79744304648461;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97387604732163;6.4269662353679]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.87165461221476;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.87236601475246;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.063698e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.508951e-121。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [0.770726236505364;-9.97166024848413;-6.66002981557571]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28109770884429;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10595076201453;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97394424694642;6.42684301022666]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86074267224782;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86145407478551;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.674772e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.128399e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [0.770726236505364;-9.97353983745558;-6.65030985946333]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98036347341231;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80521652658255;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97274226434131;6.42509832557136]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05305988906624;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05377129160394;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.896786e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.245843e-122。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [0.770726236505364;-9.97209513207065;-6.65791030397674]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.21151633500107;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.0363693881713;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455315288467;6.42793707052]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76331772212873;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76402912466642;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.516947e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.478510e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [0.770726236505364;-9.97297553170733;-6.65320809625499]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.0706523931327;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.89550544630294;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97271286771238;6.42490147599515]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05776334969445;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05847475223214;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.053873e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.503373e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [0.770726236505364;-9.97291477290992;-6.65374208401954]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.08037380071707;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.9052268538873;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97398593889089;6.42708844620324]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.85407196113291;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.8547833636706;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.065945e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.017044e-123。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [0.770726236505364;-9.9721529353073;-6.65746335048318]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.20226781713644;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.02712087030668;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97383193850533;6.42660483643641]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.87871202282215;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.87942342535985;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.678223e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.397435e-124。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [0.770726236505364;-9.97351360658589;-6.65059395583435]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98456041256163;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.80941346573187;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97282675739934;6.42518892342089]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03954099978074;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.04025240231844;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.817861e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.424564e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [0.770726236505364;-9.97166829081936;-6.66002187663529]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27981093520778;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10466398837802;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454475152079;6.4278568013334]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76466194034952;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76537334288722;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.736123e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.612158e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [0.770726236505364;-9.97360397382059;-6.65005037489333]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97010165501112;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79495470818136;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97264592664285;6.4247569008458]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06847392081884;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06918532335654;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.033756e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.095289e-125。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [0.770726236505364;-9.9719224300691;-6.65883023655585]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.23914865524839;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.06400170841863;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97408864013957;6.4271982256711]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83763976134332;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.83835116388101;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.078244e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.312182e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [0.770726236505364;-9.97316333994476;-6.65229963334835]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.04060307514326;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.8654561283135;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97371439623992;6.42636078818693]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8975187852878;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.8982301878255;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.482793e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.898345e-126。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [0.770726236505364;-9.97270314909582;-6.65489090370859]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.11423361097343;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.93908666414367;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97292219266244;6.42529594028832]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.02427135768392;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.02498276022162;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.488649e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.875162e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [0.770726236505364;-9.97235865274171;-6.65645680163259]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.16935302763113;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-1.99420608080137;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745242662128;6.42775644937598]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76793958962684;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76865099216454;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.722261e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.040462e-127。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [0.770726236505364;-9.97340377809458;-6.65124310397729]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.00213297117262;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82698602434286;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97259306859072;6.42463506561977]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0769312091603;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07764261169799;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.398391e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.229325e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [0.770726236505364;-9.97172893539363;-6.65974735738725]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27010780332434;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.09496085649458;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97418298012347;6.42729361204477]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82254536392042;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.82325676645812;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.714416e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.070771e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363234745445;-6.64997961120028]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96556187359298;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79041492676322;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97359325476087;6.42611356746046]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91690142193604;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.91761282447374;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.195123e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.558095e-128。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [0.770726236505364;-9.97178714303089;-6.65955518229041]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26079458136283;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.08564763453307;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97302692995581;6.42541680148857]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00751339074601;0.00253003695556727;0.00688343415761828;0.00252997841266994;2.00822479328371;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.984883e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.217225e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [0.770726236505364;-9.97332435912421;-6.65153104212365]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01484000643054;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.83969305960077;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97449180652154;6.4276364732903]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77313314022906;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77384454276675;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.195181e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.186326e-129。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [0.770726236505364;-9.97248454373596;-6.65606649213955]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.14921046855056;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.9740635217208;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255522808562;6.42453716322159]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08298568997511;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.0836970925128;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.377702e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.379057e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [0.770726236505364;-9.9725760615408;-6.65539209414292]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.13456761977711;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.95942067294735;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97426798345199;6.42737294845733]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80894483135678;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80965623389447;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.948156e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.714053e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [0.770726236505364;-9.97326068067894;-6.65206039869988]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.02502855767389;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.84988161084413;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97347031045711;6.42586605384765]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93657251053629;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.93728391307399;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.771754e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.332022e-130。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [0.770726236505364;-9.97183856977684;-6.65922513717599]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.25256630201037;-0.00246154585509288;-0.00729489078748485;-0.00246154585511135;-2.0774193551806;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97313917584498;6.42554869720743]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98955404847781;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.99026545101551;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.114686e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.112522e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [0.770726236505364;-9.97362486516247;-6.65009772756224]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96675904031003;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79161209348027;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97444755818994;6.42749749949915]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78021287328426;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78092427582196;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.324791e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.929399e-131。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [0.770726236505364;-9.97169737111402;-6.66004580821002]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27515808806144;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10001114123168;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253308210942;6.42446400595826]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0865290461672;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08724044870489;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.214906e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.108639e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [0.770726236505364;-9.97345387305644;-6.6509274490237]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.99411777727412;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.81897083044436;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97434285805974;6.42743487330155]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79696489411686;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79767629665455;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.057404e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.864407e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [0.770726236505364;-9.97227056328937;-6.65720984759201]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.18344734000511;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.00830039317535;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97334748270272;6.42562124427637]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.95622495124017;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.95693635377787;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.464377e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.189651e-132。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [0.770726236505364;-9.97279310144618;-6.65433071016012]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.09984123491547;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.9246942880857;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97325701800365;6.4256886301993]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.97069930309002;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.97141070562771;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.334036e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.361378e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [0.770726236505364;-9.97308777945425;-6.65302546679511]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.05269275362485;-0.00246154585509287;-0.00729489078748493;-0.00246154585511135;-1.87754580679509;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97439179797297;6.42734033630471]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78913450799996;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78984591053766;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.743706e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.217986e-133。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [0.770726236505364;-9.97199063613821;-6.65848871828178]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.22823568419071;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.05308873736095;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252703054636;6.42441600063844]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08749729625624;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08820869879394;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.824566e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.237523e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [0.770726236505364;-9.97358154727236;-6.65040357673556]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97368990272746;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79854295589769;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97440697780119;6.4274783031449]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78670573548494;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78741713802263;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.050938e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.316791e-134。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [0.770726236505364;-9.97165846891995;-6.66027655094545]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.28138243911255;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10623549228279;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97322676414437;6.42538217918951]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.97553992057471;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.9762513231124;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.901363e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.085299e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [0.770726236505364;-9.97354921692284;-6.6505034626808]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.97886275865043;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80371581182067;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97337846866835;6.42583347790695]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.95126719673905;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.95197859927674;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.020365e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.842494e-135。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [0.770726236505364;-9.97207336301832;-6.6582600397202]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.21499938337327;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.03985243654351;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.9743249120645;6.42716599007647]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79983625335483;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80054765589253;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.549472e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.972454e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [0.770726236505364;-9.97299870592059;-6.65332939774852]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.06694451900969;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.89179757217992;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253718296055;6.42439313169546]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08587290998647;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08658431252417;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.024283e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.655496e-136。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [0.770726236505364;-9.9728907774661;-6.65410688634876]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.08421307172918;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.90906612489942;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97445986222776;6.42750241100707]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77824422723348;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77895562977118;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.365769e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.548767e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [0.770726236505364;-9.97217602465778;-6.65758434922065]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.19857352106057;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.02342657423081;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97311016505649;6.42515186269363]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.9941957746368;0.00253003695556727;0.00688343415761828;0.00252997841266994;1.99490717717449;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.726620e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.876608e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [0.770726236505364;-9.97350259674722;-6.65089435894284]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.98632198674884;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.81117503991908;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97350151633571;6.42598006801513]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93157956996079;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.93229097249849;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.830015e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.480509e-137。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [0.770726236505364;-9.97167274553883;-6.66023697918164]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.27909818009173;-0.00246154585509288;-0.00729489078748484;-0.00246154585511135;-2.10395123326197;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97424741662396;6.42697568179105]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81223552384071;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.8129469263784;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.844141e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.392310e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [0.770726236505364;-9.97360917927674;-6.65026599962154]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96926878202574;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79412183519598;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97256335472665;6.42439495691146]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08168542741002;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08239682994772;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.077632e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.298456e-138。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [0.770726236505364;-9.97190455620377;-6.65915951761661]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.24200847370047;-0.00246154585509287;-0.00729489078748485;-0.00246154585511135;-2.0668615268707;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97450115552347;6.42750660233647]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77163729991975;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77234870245744;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.144719e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.107920e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [0.770726236505364;-9.97318368390319;-6.65243569776654]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.03734804179489;-0.00246154585509287;-0.00729489078748493;-0.00246154585511134;-1.86220109496513;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97299965789015;6.42493318568801]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01187692125004;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.01258832378773;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.482499e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.078930e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [0.770726236505364;-9.97267780352521;-6.65526158025228]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.11828890227116;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.94314195544139;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97362418185713;6.42612526185841]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91195308653335;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.91266448907104;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.590532e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.227972e-139。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [0.770726236505364;-9.97238371570685;-6.65656791815728]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.16534295320842;-0.00246154585509287;-0.00729489078748488;-0.00246154585511135;-1.99019600637865;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97415997876325;6.42677086255504]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82622558155489;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.82693698409258;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.791631e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.760972e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [0.770726236505364;-9.97338877612029;-6.65156382793809]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.00453328705788;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82938634022811;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726050734756;6.42442061722914]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07501042757937;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07572183011706;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.483736e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.991484e-140。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [0.770726236505364;-9.97173935084814;-6.65993229861652]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.26844133060291;-0.00246154585509287;-0.00729489078748484;-0.00246154585511135;-2.09329438377315;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453060634192;6.42749049089147]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76692516896712;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76763657150482;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.701480e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.433816e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [0.770726236505364;-9.97363336411667;-6.65021732980023]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.96539920763725;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79025226080749;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97289712747488;6.42472885965933]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.02828178769337;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.02899319023106;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.262488e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.022389e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [0.770726236505364;-9.971774229298;-6.65985881375571]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.2628607786238;-0.00246154585509287;-0.00729489078748484;-0.00246154585511135;-2.08771383179403;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97374457312223;6.42626603707583]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.89269048411771;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.8934018866554;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.086072e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.055165e-141。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [0.770726236505364;-9.97334120729489;-6.65168538430471]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.01214429912265;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.83699735229289;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.974063436205;6.42655322683675]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84167239087524;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.84238379341294;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.208816e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.702369e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [0.770726236505364;-9.97245914238573;-6.65643641563389]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.15327468458697;-0.00246154585509287;-0.00729489078748489;-0.00246154585511135;-1.97812773775721;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97266159427991;6.42446885836562]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06596709888863;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06667850142633;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.257827e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.245372e-142。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [0.770726236505364;-9.97260167063165;-6.6555005911312]
Robot Arm 1 Joint Angles: [-0.00246154585509859;2.13047016524001;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.95532321841025;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454805015009;6.42745387680449]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76413415966036;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76484556219806;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.051227e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.271440e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [0.770726236505364;-9.9732419975253;-6.65239953661904]
Robot Arm 1 Joint Angles: [-0.00246154585509858;2.02801786225706;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.8528709154273;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97280433000014;6.42454136504284]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04312938365187;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04384078618957;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.914939e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.376325e-143。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [0.770726236505364;-9.97185431653843;-6.65938315179443]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.0943951023932;-0.00246154399244773;-0.00729489265013;-0.0024615439924662;-2.07489987146313;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97386093234155;6.42639956058074]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.87407300902733;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.87478441156502;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.220075e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.405967e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [0.770726236505364;-9.97362169879457;-6.6503574255402]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.81161394141005;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.79211871047998;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97395881355364;6.42632472167339]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.85841201509304;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.85912341763074;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.132444e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.000262e-144。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [0.770726236505364;-9.97173493167489;-6.66012758132594]
Robot Arm 1 Joint Angles: [-0.00246154399245346;2.11349668055974;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.09400144962968;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97273192024305;6.42453805936109]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0547149447869;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.0554263473246;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.689824e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.101169e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [0.770726236505364;-9.97318565987713;-6.65254015451955]
Robot Arm 1 Joint Angles: [-0.00246154399245344;1.88138016820063;-0.00246154399244772;-0.00729489265013008;-0.00246154399246619;-1.86188493727056;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455339552448;6.42739672906316]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7632788997575;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7639903022952;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.413461e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.460329e-145。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [0.770726236505364;-9.97273712259859;-6.65507258293915]
Robot Arm 1 Joint Angles: [-0.00246154399245344;1.95314613276776;-0.00246154399244772;-0.00729489265013005;-0.0024615439924662;-1.93365090183769;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.9727228608282;6.42437291330736]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05616445116222;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05687585369992;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.790697e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.677597e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [0.770726236505364;-9.97208511805578;-6.65822960464205]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.05746685961697;-0.00246154399244772;-0.00729489265013001;-0.0024615439924662;-2.0379716286869;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97397167146795;6.42652324519105]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8563547488019;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.8570661513396;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.332303e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.695120e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [0.770726236505364;-9.97356322966403;-6.65074146459399]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.82096900229756;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.80147377136749;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.973847332712;6.42608754914432]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.87624894975398;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.87696035229167;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.380339e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.526493e-146。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [0.770726236505364;-9.97163016662804;-6.66068014233543]
Robot Arm 1 Joint Angles: [-0.00246154399245346;2.13025908805555;-0.00246154399244773;-0.00729489265012998;-0.0024615439924662;-2.11076385712548;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97281482503264;6.42462626307945]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04145017845217;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04216158098987;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.039421e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.454633e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [0.770726236505364;-9.97339071048593;-6.65154268552174]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.84857207079352;-0.00246154399244772;-0.0072948926501301;-0.00246154399246619;-1.82907683986345;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745466155716;6.42731917423712]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76436369221823;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76507509475592;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.591870e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.899045e-147。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [0.770726236505364;-9.97244233467541;-6.65663250983198]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.00031220047633;-0.00246154399244772;-0.00729489265013003;-0.0024615439924662;-1.98081696954626;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97265412865529;6.42422541819026]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06716159882878;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06787300136647;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.808968e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.021719e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [0.770726236505364;-9.97236146818822;-6.65684713628604]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.01325083842606;-0.00246154399244772;-0.00729489265013003;-0.0024615439924662;-1.99375560749599;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97407539377896;6.42663478697817]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83975917904016;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.84047058157786;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.424133e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.132053e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [0.769679760620695;-9.97343704627027;-6.65147450954606]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.84115834716112;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.82166311623105;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97373041459993;6.42584415836266]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.89495584768611;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.89566725022381;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.323822e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.203263e-148。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [0.769679760620695;-9.97161715775937;-6.66076321913807]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.13234050890509;-0.00246154585509288;-0.00729489078748483;-0.00246154585511135;-2.11284527797502;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97290887545791;6.42473120608714]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.02640211040941;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.02711351294711;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.891461e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.788938e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [0.769679760620695;-9.97353496255036;-6.65086460502305]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.82549174234668;-0.00246154585509287;-0.00729489078748495;-0.00246154585511134;-1.80599651141661;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97452774523165;6.4272214924633]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76738294661137;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76809434914906;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.810917e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.525153e-149。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [0.769679760620695;-9.97215813857285;-6.65812446730694]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.04578357874788;-0.00246154585509287;-0.00729489078748487;-0.00246154585511135;-2.02628834781781;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97259933258288;6.4241004702953]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.075928970413;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.0766403729507;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.201089e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.254054e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [0.769679760620695;-9.97265612557475;-6.65537480157476]
Robot Arm 1 Joint Angles: [-0.00246154585509859;1.96610565844368;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.94661042751361;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97417090209239;6.42673218401573]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82447784889236;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.82518925143006;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.881476e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.652145e-150。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [0.769679760620695;-9.97324796661223;-6.65252700131473]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.87141109244826;-0.00246154585509287;-0.00729489078748494;-0.00246154585511134;-1.85191586151819;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97360966935085;6.42559722289551]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9142750875381;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9149864900758;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.436317e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.826317e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [0.769679760620695;-9.97169723807377;-6.66037279225543]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.1195276586013;-0.00246154585509287;-0.00729489078748484;-0.00246154585511135;-2.10003242767123;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97301245465581;6.42485034908991]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00982943874531;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.010540841283;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.201834e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.709508e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [0.769679760620695;-9.97361246564439;-6.6505376080045]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.81309124730146;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79359601637139;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9744968847069;6.42710412063691]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77232063057101;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77303203310871;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.559285e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.105332e-151。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [0.769679760620695;-9.97191220338996;-6.65940975693025]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.08513320801082;-0.00246154585509287;-0.00729489078748485;-0.00246154585511135;-2.06563797708075;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255943958756;6.42399931092582]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08231184966514;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08302325220283;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.154340e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.055566e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [0.769679760620695;-9.97294175069819;-6.65395313221477]
Robot Arm 1 Joint Angles: [-0.00246154585509859;1.92040563869434;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.90091040776427;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97425719577732;6.42681373961347]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81067085930361;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.81138226184131;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.040571e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.760825e-152。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [0.769679760620695;-9.97300558809728;-6.65384431956514]
Robot Arm 1 Joint Angles: [-0.00246154585509859;1.91019165483932;-0.00246154585509287;-0.00729489078748492;-0.00246154585511134;-1.89069642390926;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97348687298477;6.4253496006011]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.933922506111;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9346339086487;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.635945e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.351811e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [0.769679760620695;-9.97186220733625;-6.65955195973862]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.09313257660487;-0.00246154585509287;-0.00729489078748485;-0.00246154585511135;-2.0736373456748;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97312378859001;6.4249809123541]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.99201600927335;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.99272741181104;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.011070e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.992413e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [0.769679760620695;-9.97362075507554;-6.65057365685102]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.81176493831843;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.79226970738836;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97445420870371;6.42696766172855]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77914879108089;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77986019361859;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.134772e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.267077e-153。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [0.769679760620695;-9.97172926522712;-6.66036545217028]
Robot Arm 1 Joint Angles: [-0.00246154585509861;2.1144033140652;-0.00246154585509287;-0.00729489078748484;-0.00246154585511135;-2.09490808313513;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253516212571;6.42392280462032]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08619624356164;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08690764609934;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.348769e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.233362e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [0.769679760620695;-9.97319472014245;-6.65270386904317]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.87993052761301;-0.00246154585509287;-0.00729489078748493;-0.00246154585511134;-1.86043529668294;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.9743334594286;6.42687805398006]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79846867509852;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79918007763622;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.809704e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.204965e-154。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [0.769679760620695;-9.9727258039818;-6.65534097431552]
Robot Arm 1 Joint Angles: [-0.00246154585509859;1.95495711331605;-0.00246154585509287;-0.0072948907874849;-0.00246154585511135;-1.93546188238598;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97336393036025;6.42510427560604]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.95359332603453;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.95430472857223;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.321547e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.416802e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [0.769679760620695;-9.9720952466377;-6.65838712995819]
Robot Arm 1 Joint Angles: [-0.0024615458550986;2.05584628837255;-0.00246154585509287;-0.00729489078748486;-0.00246154585511135;-2.03635105744248;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34529194213667;9.97324097927737;6.4251199216141]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.97326549929491;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.9739769018326;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.631619e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.506012e-155。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [0.769679760620695;-9.97355959964103;-6.65097163059773]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.82154980783953;-0.00246154585509287;-0.00729489078748496;-0.00246154585511134;-1.80205457690947;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34529194213667;9.97439998066911;6.42681289844535]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78782527661756;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78853667915525;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.510830e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.957436e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [0.763865188453691;-9.97162794109504;-6.66090035516492]
Robot Arm 1 Joint Angles: [-0.00246154399245346;2.00699104113108;-0.00246154399244773;-0.00729489265012998;-0.0024615439924662;-2.0943951010182;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252693802932;6.42387141361671]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08751209898365;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08822350152135;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.429736e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.668883e-156。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [0.763865188453691;-9.97339092280563;-6.65174944412486]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.72491396743753;-0.00246154399244772;-0.00729489265013009;-0.00246154399246619;-1.81231802732465;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97439904601943;6.42692400797131]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78797482056598;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78868622310367;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.391206e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.479716e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [0.763865188453691;-9.97263762127236;-6.655850375715]
Robot Arm 1 Joint Angles: [-0.00246154399245344;1.84544221276084;-0.00246154399244772;-0.00729489265013005;-0.0024615439924662;-1.93284627264796;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97324282670862;6.42486428596831]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.97296991029496;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.97368131283266;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.165413e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.347711e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [0.763865188453691;-9.97209335942806;-6.65844862862402]
Robot Arm 1 Joint Angles: [-0.00246154399245345;1.93252410784878;-0.00246154399244772;-0.00729489265013001;-0.0024615439924662;-2.0199281677359;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97336204697686;6.42526426817134]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.95389466737754;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.95460606991523;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.634898e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.170611e-157。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [0.763865188453691;-9.97364081561979;-6.65059725376986]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.68493111717189;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.77233517705902;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97433457080682;6.42664080923766]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79829085458372;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79900225712141;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.612295e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.184751e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [0.763865188453691;-9.97171825022105;-6.66052633021682]
Robot Arm 1 Joint Angles: [-0.00246154399245346;1.99254158096995;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.07994564085707;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253491623533;6.42384517959569]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08623558602227;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08694698855997;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.567196e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.289508e-158。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [0.763865188453691;-9.97316032005488;-6.65298065755305]
Robot Arm 1 Joint Angles: [-0.00246154399245344;1.76181040755659;-0.00246154399244772;-0.00729489265013008;-0.00246154399246619;-1.84921446744371;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97445345694784;6.42695074186132]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77926907202114;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77998047455883;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.449579e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.127760e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [0.763865188453691;-9.97294043789669;-6.65434354719332]
Robot Arm 1 Joint Angles: [-0.00246154399245344;1.79699155286661;-0.00246154399244772;-0.00729489265013007;-0.0024615439924662;-1.88439561275373;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97312557247003;6.4246326441641]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.99173058847083;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.99244199100853;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.490736e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.203810e-159。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [0.763865188453691;-9.97185520768757;-6.65974585361818]
Robot Arm 1 Joint Angles: [-0.00246154399245345;1.97062838632719;-0.00246154399244772;-0.00729489265013;-0.0024615439924662;-2.05803244621431;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34529194213667;9.97348498091253;6.42541078283136]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93422523767027;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.93493664020796;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.290978e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.368236e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [0.763865188453691;-9.97367156938554;-6.65045789088923]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.6800105146508;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.76741457453792;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34529194213667;9.97425847640138;6.42645258486659]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8104659594542;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.8111773619919;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.516625e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.491996e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [0.767349820442101;-9.97189809522328;-6.65969049131292]
Robot Arm 1 Joint Angles: [-0.00246154212980831;1.86545174732375;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.05117043838616;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255895155162;6.42384371754647]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08238993541651;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08310133795421;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.962399e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.053416e-160。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [0.767349820442101;-9.97288086619128;-6.6544693994126]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.70820839244293;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.89392708350533;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34529194213667;9.97449632099428;6.42695763151633]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77241082458989;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77312222712759;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.208772e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.545384e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [0.767349820442101;-9.9733972801356;-6.65202404909303]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.62558216135207;-0.00246154212980257;-0.00729489451277524;-0.00246154212982104;-1.81130085241448;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34529194213667;9.97301414749839;6.42441225935771]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00955858393321;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.0102699864709;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.811328e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.011290e-161。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165979310339;-6.66086349995406]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81102339461718;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08929877549779;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97360779505512;6.425556318666]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91457497485612;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.91528637739381;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.606094e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.735828e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [0.767349820442101;-9.97353732872561;-6.65115253807024]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.51061769506165;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78889307594226;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9741723428292;6.42624964412342]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82424733100352;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.82495873354122;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.701444e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.425767e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [0.767349820442101;-9.97282594263663;-6.65504524938726]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.62443946929826;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.90271485017888;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97259860975261;6.42386622373594]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0760446232571;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.0767560257948;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.150864e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.310206e-162。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [0.767349820442101;-9.9718734934416;-6.65975810734763]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77683134050382;-0.00246154026715743;-0.0072948963754203;-0.0024615402671759;-2.05510672138443;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97452737395472;6.42694426417392]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76744235092031;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76815375345801;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.127273e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.462258e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [0.767349820442101;-9.97375334331672;-6.65012362849015]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.476055360484;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.75433074136461;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97291045028835;6.42420586947582]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.02615013753951;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.0268615400772;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.055619e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.314499e-163。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [0.767349820442101;-9.97230041342623;-6.65777383375311]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.70852414296196;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.98679952384258;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9737285833264;6.42569783426202]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.89524885145173;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.89596025398942;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.733900e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.613816e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [0.767349820442101;-9.97230075762148;-6.65757145835301]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.70846907172253;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.98674445260314;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97407698349457;6.4260336474442]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83950482454461;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.84021622708231;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.009523e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.736094e-164。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737532871845;-6.65022524384498]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47606434163852;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.75433972251913;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97265318175184;6.42391149597346]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06731310338037;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.06802450591807;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.936851e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.146190e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [0.767349820442101;-9.97187329131314;-6.65996646205053]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77686368105749;-0.00246154026715743;-0.0072948963754203;-0.0024615402671759;-2.0551390619381;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454644057011;6.42691041609354]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.7643916924575;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7651030949952;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.617712e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.711169e-165。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [0.767349820442101;-9.97282631891294;-6.65489758714148]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.62437926508813;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.90265464596875;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97281625573183;6.42401598758993]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04122126658304;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04193266912073;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.272289e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.905251e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [0.767349820442101;-9.97353715930487;-6.65144182183274]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.51064480238011;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78892018326072;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97384556815599;6.42583246840807]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.87653127871711;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.87724268125481;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.355701e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.867820e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165977230299;-6.66104766858649]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81102672268122;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08930210356183;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97397339667969;6.42580650678492]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8560787149248;0.00253003695556726;0.00688343415761832;0.00252997841266993;1.85679011746249;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.906605e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.279889e-166。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 400:
Mobile Platform 1 Pose: [0.767349820442101;-9.97331112367778;-6.65245167428361]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.54681050271395;-0.00246154026715742;-0.00729489637542039;-0.0024615402671759;-1.82508588359456;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97272170367081;6.4239779615275]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05634959634646;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05706099888416;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.795023e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.272956e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 401:
Mobile Platform 1 Pose: [0.767349820442101;-9.97313312544785;-6.65360528354869]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.57529021950229;-0.00246154026715742;-0.00729489637542037;-0.0024615402671759;-1.8535656003829;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455342022357;6.42685603433413]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76327494790363;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76398635044133;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.639055e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.187082e-167。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 402:
Mobile Platform 1 Pose: [0.767349820442101;-9.97171344007321;-6.66075782678279]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80243987944519;-0.00246154026715743;-0.00729489637542029;-0.0024615402671759;-2.0807152603258;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97273318186587;6.42384486238638]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05451308513666;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05522448767436;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.218810e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.234822e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 403:
Mobile Platform 1 Pose: [0.767349820442101;-9.97364710685455;-6.65079195966248]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4930531944303;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.77132857531092;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97395713765117;6.42595759914784]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.85868015948722;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.85939156202492;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.222876e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.413169e-168。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 404:
Mobile Platform 1 Pose: [0.767349820442101;-9.97261664651733;-6.65631071546245]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.65792684838532;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.93620222926593;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97386277690304;6.4255703891997]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8737778791893;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.874489281727;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.623004e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.726589e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 405:
Mobile Platform 1 Pose: [0.767349820442101;-9.97202126813093;-6.65917461673567]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.75318739020991;-0.00246154026715743;-0.0072948963754203;-0.0024615402671759;-2.03146277109052;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97280297925672;6.42406370757019]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04334550260016;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04405690513786;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.032751e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.534779e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 406:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737796197437;-6.65020108007291]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4718511321666;-0.00246154026715742;-0.00729489637542042;-0.00246154026717589;-1.75012651304721;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454827762116;6.42678122456571]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76409776429006;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76480916682775;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.455424e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.823559e-169。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 407:
Mobile Platform 1 Pose: [0.767349820442101;-9.97211326050344;-6.65891192801421]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.73846861060892;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.01674399148953;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97266266339614;6.42369444853124]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06579604029271;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06650744283041;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.864253e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.504977e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 408:
Mobile Platform 1 Pose: [0.767349820442101;-9.97250351150398;-6.656712459674]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.67602845052255;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.95430383140316;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97406186905053;6.42607088373338]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84192313559002;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.84263453812772;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.339572e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.707944e-170。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 409:
Mobile Platform 1 Pose: [0.767349820442101;-9.97369336509837;-6.65075000848838]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48565187542044;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.76392725630105;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97374651798189;6.42532771049494]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.89237930657261;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.8930907091103;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.682540e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.632531e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 410:
Mobile Platform 1 Pose: [0.767349820442101;-9.97175941297136;-6.6607205281713]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79508421574157;-0.00246154026715743;-0.00729489637542029;-0.0024615402671759;-2.07335959662218;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97289560246345;6.42416651113828]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.02852578952385;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.02923719206154;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.410081e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.445463e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 411:
Mobile Platform 1 Pose: [0.767349820442101;-9.97302896102742;-6.6540462636062]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.59195652677266;-0.00246154026715742;-0.00729489637542037;-0.0024615402671759;-1.87023190765328;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453103927775;6.42668624605703]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76685589923456;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76756730177226;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.972690e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.579419e-171。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 412:
Mobile Platform 1 Pose: [0.767349820442101;-9.9733978334026;-6.65237003633346]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.53293694674386;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.81121232762447;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726059285765;6.42356638113696]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07487361143497;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07558501397267;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.216525e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.356948e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 413:
Mobile Platform 1 Pose: [0.767349820442101;-9.97164828954057;-6.66127467015179]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81286396466786;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.09113934554847;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97415853863694;6.42617027972571]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82645600176479;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.82716740430249;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.492344e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.729592e-172。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 414:
Mobile Platform 1 Pose: [0.767349820442101;-9.97346517235206;-6.65185804187141]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.52216271482867;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.80043809570928;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97362620493582;6.42508111484862]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91162939394411;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.91234079648181;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.064037e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.201908e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 415:
Mobile Platform 1 Pose: [0.767349820442101;-9.97293746615481;-6.65481013829029]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60659570638896;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.88487108726957;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97299798041812;6.42428386927017]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01214531677578;0.00253003695556727;0.00688343415761827;0.00252997841266994;2.01285671931348;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.982265e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.742109e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 416:
Mobile Platform 1 Pose: [0.767349820442101;-9.97180710445728;-6.66044855337725]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78745357799347;-0.00246154026715743;-0.00729489637542029;-0.0024615402671759;-2.06572895887408;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97450179612292;6.42657151393509]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77153480400832;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77224620654601;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.597997e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.393076e-173。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 417:
Mobile Platform 1 Pose: [0.767349820442101;-9.97372498528675;-6.65059900958072]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48059264527878;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.75886802615939;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97256397671206;6.42346194983644]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08158590974594;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08229731228363;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.328269e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.580231e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 418:
Mobile Platform 1 Pose: [0.767349820442101;-9.97240926943209;-6.65756044948876]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.69110718202512;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.96938256290573;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97424612010065;6.42625405009344]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81244296757148;0.00253003695556725;0.00688343415761833;0.00252997841266993;1.81315437010918;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.517304e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.143392e-174。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 419:
Mobile Platform 1 Pose: [0.767349820442101;-9.97219660083925;-6.65845224429889]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.72513415687924;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-2.00340953775985;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9735035925622;6.42483343714062]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93124737372272;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.93195877626042;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.876158e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.241043e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 420:
Mobile Platform 1 Pose: [0.767349820442101;-9.973771856407;-6.65045602036273]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47309326603933;-0.00246154026715742;-0.00729489637542042;-0.00246154026717589;-1.75136864691994;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97310835925098;6.42441303340171]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.99448470351841;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.99519610605611;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.198752e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.276291e-175。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 421:
Mobile Platform 1 Pose: [0.767349820442101;-9.97194979452188;-6.65992452974083]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76462316765773;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04289854853834;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9744607119894;6.42643760776253]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77810826537006;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77881966790776;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.747900e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.331297e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 422:
Mobile Platform 1 Pose: [0.767349820442101;-9.97271290560077;-6.65582210737838]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.64252539503592;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.92080077591653;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253755568037;6.42338207139433]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0858132748158;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08652467735349;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.052564e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.047932e-176。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 423:
Mobile Platform 1 Pose: [0.767349820442101;-9.97360074925743;-6.65144214311437]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50047040997065;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.77874579085126;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97432377417572;6.42632075621606]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80001831555942;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80072971809711;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.125938e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.793904e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 424:
Mobile Platform 1 Pose: [0.767349820442101;-9.97168382091112;-6.66127678950016]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80717894537978;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08545432626039;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97338057019015;6.4245876472622]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.95093095325119;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.95164235578889;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.781651e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.628323e-177。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 425:
Mobile Platform 1 Pose: [0.767349820442101;-9.97321720307031;-6.65327190477874]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56183779990917;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.84011318078979;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97322485621401;6.42455105353932]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.9758451894327;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.97655659197039;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.243167e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.611315e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 426:
Mobile Platform 1 Pose: [0.767349820442101;-9.97323244359819;-6.65342787782967]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.55939931544907;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.83767469632968;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97440803722931;6.4262852847204]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78653622698535;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78724762952305;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.149613e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.204346e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 427:
Mobile Platform 1 Pose: [0.767349820442101;-9.97167922815209;-6.6612848134243]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80791378682483;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08618916770545;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252714133536;6.42332726363261]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08747957001732;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08819097255502;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.578071e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.682167e-178。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 428:
Mobile Platform 1 Pose: [0.767349820442101;-9.97359134168351;-6.65141122794387]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50197562179728;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78025100267789;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97439083239895;6.42636924252521]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78928899984324;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79000040238093;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.843221e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.464719e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 429:
Mobile Platform 1 Pose: [0.767349820442101;-9.97273086641217;-6.65606508689907]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.63965166521245;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.91792704609307;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97325911455947;6.42434677940941]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.97036385416053;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.97107525669823;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.801554e-181。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.542506e-179。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 430:
Mobile Platform 1 Pose: [0.767349820442101;-9.97193714384696;-6.65995478420396]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76664727564512;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04492265652573;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97334550058611;6.4246948363156]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.956542089897;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.9572534924347;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.743625e-181。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.442256e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 431:
Mobile Platform 1 Pose: [0.767349820442101;-9.97376962977754;-6.65058035029115]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47344952675233;-0.00246154026715742;-0.00729489637542042;-0.00246154026717589;-1.75172490763294;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97434412628893;6.42611549539937]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79676197744576;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79747337998346;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.980642e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.375034e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 432:
Mobile Platform 1 Pose: [0.767349820442101;-9.97221269441162;-6.65874944799337]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.7225591852995;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-2.00083456618011;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253292224046;6.42329762587446]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08655462520145;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08726602773915;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.392354e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.435107e-180。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 433:
Mobile Platform 1 Pose: [0.767349820442101;-9.97239192225322;-6.65762993591526]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.6938827306428;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.97215811152341;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97444677746437;6.42639861581615]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78033778937634;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78104919191404;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.025899e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.935140e-181。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 434:
Mobile Platform 1 Pose: [0.767349820442101;-9.97373018166275;-6.65088673383784]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47976122511967;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.75803660600028;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97314123525506;6.42411385307874]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.98922454286584;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.98993594540353;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.529216e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.107561e-181。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 435:
Mobile Platform 1 Pose: [0.767349820442101;-9.97181694673092;-6.66077709993993]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78587881421198;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06415419509259;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9734682833487;6.42484121720243]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93689684788266;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.93760825042035;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.605900e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.931851e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 436:
Mobile Platform 1 Pose: [0.767349820442101;-9.97292012905874;-6.65494569642128]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60936964176081;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.88764502264142;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97426945779921;6.42592940037019]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80870893580204;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80942033833974;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.334009e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.263353e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 437:
Mobile Platform 1 Pose: [0.767349820442101;-9.97347721018151;-6.6522910216645]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.52023666211698;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.7985120429976;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255479308275;6.42329283097319]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08305529043501;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08376669297271;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.251901e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.278782e-182。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 438:
Mobile Platform 1 Pose: [0.767349820442101;-9.97164935059942;-6.6616216302119]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81269419525148;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.09096957613209;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97449122223499;6.4264082216604]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77322662607698;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77393802861467;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.791622e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.908645e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 439:
Mobile Platform 1 Pose: [0.767349820442101;-9.97338465968864;-6.6526075372977]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.53504474097544;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.81332012185605;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97302891867076;6.42389179462075]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.00719519635409;0.00253003695556727;0.00688343415761828;0.00252997841266994;2.00790659889178;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.724397e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.315345e-183。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 440:
Mobile Platform 1 Pose: [0.767349820442101;-9.97304586096801;-6.65458928563159]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.5892525362771;-0.00246154026715742;-0.00729489637542037;-0.0024615402671759;-1.86752791715771;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97359121254588;6.42498704246566]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91722817633518;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.91793957887288;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.182094e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.876630e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 441:
Mobile Platform 1 Pose: [0.767349820442101;-9.9717514346307;-6.6610846524]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79636075024582;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07463613112643;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97418465557919;6.42572838607929]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.82227729100478;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.82298869354248;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.201152e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.565859e-184。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 442:
Mobile Platform 1 Pose: [0.767349820442101;-9.97368674431612;-6.65112621315809]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48671120057918;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.76498658145979;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97259235838293;6.42331213138824]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07704484240654;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07775624494424;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.794891e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.631464e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 443:
Mobile Platform 1 Pose: [0.767349820442101;-9.97252145490253;-6.65732841522139]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.6731575067536;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.95143288763421;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97452388919987;6.42639762012374]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76799991169591;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7687113142336;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.373997e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.207497e-185。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 444:
Mobile Platform 1 Pose: [0.767349820442101;-9.97209825960415;-6.65930398083314]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.74086875449513;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.01914413537574;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97292407631844;6.42368336766309]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.02396997272565;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.02468137526335;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.493207e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.552459e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 445:
Mobile Platform 1 Pose: [0.767349820442101;-9.97378039565037;-6.65073877874291]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47172698710001;-0.00246154026715741;-0.00729489637542042;-0.00246154026717589;-1.75000236798062;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97371236890026;6.42512925284293]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.89784315963447;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.89855456217216;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.320838e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.148581e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 446:
Mobile Platform 1 Pose: [0.767349820442101;-9.97203548179484;-6.65983462286255]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.75091320398353;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.02918858486414;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97409050881555;6.42551407882026]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83734077318697;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.83805217572467;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.777787e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.116191e-186。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 447:
Mobile Platform 1 Pose: [0.767349820442101;-9.97259870716031;-6.65675149257941]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.66079714550982;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.93907252639044;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97264494558556;6.42335437800046]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0686308899868;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.0693422925245;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.148346e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.334653e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 448:
Mobile Platform 1 Pose: [0.767349820442101;-9.97365523276826;-6.65148860682208]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49175304823643;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.77002842911704;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454459201309;6.42636656304173]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76468746158065;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76539886411834;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.462306e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.721971e-187。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 449:
Mobile Platform 1 Pose: [0.767349820442101;-9.97172010950177;-6.66144300190421]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80137277087545;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07964815175606;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97282850135664;6.42349111749175]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03926196661286;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.03997336915056;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.621518e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.767119e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 450:
Mobile Platform 1 Pose: [0.767349820442101;-9.9731169889941;-6.65412541883522]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.57787205210219;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.8561474329828;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97382995556635;6.42526495983054]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.87902929306005;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.87974069559774;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.610172e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.663497e-188。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 451:
Mobile Platform 1 Pose: [0.767349820442101;-9.97332557014471;-6.65328069577002]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.54449906800525;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.82277444888586;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97398798945221;6.42528835523566]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8537438713215;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.8544552738592;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.921318e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.195658e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 452:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165723713814;-6.66174954196706]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81143234905575;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08970772993636;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97271162453534;6.4234180472933]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.0579622580209;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.0586736605586;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.731260e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.435540e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 453:
Mobile Platform 1 Pose: [0.767349820442101;-9.97352637197342;-6.65207889076659]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.51237077541117;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.79064615629178;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455322060937;6.42631497512347]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76330688617593;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76401828871363;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.826323e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.383275e-189。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 454:
Mobile Platform 1 Pose: [0.767349820442101;-9.97284437289687;-6.65582132340547]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.62149062765954;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.89976600854016;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97274383422103;6.42331732981094]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05280870831081;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05352011084851;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.316888e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.020174e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 455:
Mobile Platform 1 Pose: [0.767349820442101;-9.97186216530038;-6.66068875848506]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77864384309816;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.05691922397877;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97394233702085;6.42539150711659]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86104826034032;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86175966287801;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.891212e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.912984e-190。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 456:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737495716812;-6.65101224217267]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47665882216631;-0.00246154026715742;-0.00729489637542042;-0.00246154026717589;-1.75493420304692;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97387826444607;6.42505334694149]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.87129987230515;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.87201127484285;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.881951e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.202241e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 457:
Mobile Platform 1 Pose: [0.767349820442101;-9.97231800271873;-6.6585555096129]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.70570985616122;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.98398523704183;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97279122973687;6.4235012716835]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04522542577725;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04593682831495;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.038489e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.292702e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 458:
Mobile Platform 1 Pose: [0.767349820442101;-9.97228361123359;-6.65853139767998]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.71121249378487;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.98948787466549;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454973115431;6.42624294086183]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76386519898595;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76457660152365;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.479849e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.147892e-191。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 459:
Mobile Platform 1 Pose: [0.767349820442101;-9.97375715680155;-6.65107407324757]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47544520291124;-0.00246154026715742;-0.00729489637542042;-0.00246154026717589;-1.75372058379185;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97267153543869;6.42316400011922]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06437651348507;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06508791602276;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.173143e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.892566e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 460:
Mobile Platform 1 Pose: [0.767349820442101;-9.9718852616031;-6.66077780959247]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77494843466168;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.0532238155423;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97404806440795;6.42550651316737]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84413187840297;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.84484328094067;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.137767e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.776713e-192。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 461:
Mobile Platform 1 Pose: [0.767349820442101;-9.97280807624321;-6.65586253716593]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.62729809224489;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.9055734731255;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97376270013064;6.42481143577953]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88979016277356;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.89050156531126;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.382307e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.845105e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 462:
Mobile Platform 1 Pose: [0.767349820442101;-9.9735483151619;-6.65225342959259]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50885986525493;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78713524613554;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97288238294387;6.42360186957809]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03064091265617;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03135231519387;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.288995e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.579643e-193。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 463:
Mobile Platform 1 Pose: [0.767349820442101;-9.97166301343793;-6.66190407141531]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81050814108989;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08878352197051;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453414171147;6.42615069851684]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76635950984001;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7670709123777;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.781946e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.886895e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 464:
Mobile Platform 1 Pose: [0.767349820442101;-9.97329638612134;-6.65339793130275]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.54916851174499;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.8274438926256;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97261286226168;6.42303280795756]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07376422180731;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07447562434501;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.105080e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.146451e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 465:
Mobile Platform 1 Pose: [0.767349820442101;-9.97314993221916;-6.65438887110808]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.57260113609371;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.85087651697432;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97414588720234;6.42560789475465]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8284802313011;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.8291916338388;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.872358e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.129090e-194。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 466:
Mobile Platform 1 Pose: [0.767349820442101;-9.97170717030645;-6.66166282344814]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80344304212715;-0.00246154026715743;-0.00729489637542029;-0.0024615402671759;-2.08171842300776;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97364285579892;6.42456523559121]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90896525584886;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.90967665838656;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.332524e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.917426e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 467:
Mobile Platform 1 Pose: [0.767349820442101;-9.97363876963449;-6.65170451729199]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4943871496406;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.77266253052121;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97298351595288;6.42371737528849]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.01445963121403;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.01517103375173;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.494047e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.120956e-195。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 468:
Mobile Platform 1 Pose: [0.767349820442101;-9.97263553924596;-6.65708496030668]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.65490401180428;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.93317939268489;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97450653401434;6.42603864141767]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77077674138103;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77148814391873;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.593875e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.902023e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 469:
Mobile Platform 1 Pose: [0.767349820442101;-9.97200702702772;-6.66012002168031]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.755465966723;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.03374134760361;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97256884619213;6.42292509120639]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08080679293542;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08151819547311;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.465651e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.642795e-196。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 470:
Mobile Platform 1 Pose: [0.767349820442101;-9.97377883280369;-6.6510740745532]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47197704256783;-0.00246154026715742;-0.00729489637542042;-0.00246154026717589;-1.75025242344845;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97423475298897;6.42569387400891]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81426170543997;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.81497310797767;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.115602e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.431295e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 471:
Mobile Platform 1 Pose: [0.767349820442101;-9.97212931358236;-6.65970190410241]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.73590011798108;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.01417549886169;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97352046418697;6.42431755704631]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.928547913761;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9292593162987;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.143785e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.291731e-197。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 472:
Mobile Platform 1 Pose: [0.767349820442101;-9.97248513974465;-6.65767846373576]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.67896793201431;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.95724331289492;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.973092895825;6.42384507249934]
Robot Arm 2 Joint Angles: [0.00253002436772006;-1.99695885167481;0.00253003695556727;0.00688343415761828;0.00252997841266994;1.9976702542125;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.667065e-199。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.181435e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 473:
Mobile Platform 1 Pose: [0.767349820442101;-9.97370026052675;-6.6515832544005]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48454860687923;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.76282398775984;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97446706118103;6.42590732576183]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77709239471122;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77780379724892;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.610807e-199。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.484894e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 474:
Mobile Platform 1 Pose: [0.767349820442101;-9.97176828298799;-6.66154797960585]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79366501308067;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07194039396128;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254027045538;6.42284181882727]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08537891081429;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08609031335199;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.976222e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.298058e-198。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 475:
Mobile Platform 1 Pose: [0.767349820442101;-9.97301144278812;-6.65500724231872]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.59475944505917;-0.00246154026715742;-0.00729489637542037;-0.0024615402671759;-1.87303482593978;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97431379812273;6.42576297284372]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.801614484038;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.8023258865757;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.795630e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.045348e-199。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 476:
Mobile Platform 1 Pose: [0.767349820442101;-9.97341164288117;-6.65316831646966]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.53072743017198;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.80900281105259;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97339739808076;6.42407135435992]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.94823849075411;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.94894989329181;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.626027e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.890948e-199。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 477:
Mobile Platform 1 Pose: [0.767349820442101;-9.97164775278943;-6.66215044980616]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81294984484956;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.09122522573017;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97320865593645;6.42398203677302]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.97843723384379;0.00253003695556727;0.00688343415761828;0.00252997841266994;1.97914863638149;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.125616e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.138100e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 478:
Mobile Platform 1 Pose: [0.767349820442101;-9.97345258640748;-6.6527929356389]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.52417646596184;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.80245184684245;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97441596068296;6.42575748327787]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78526847440098;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.78597987693868;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.271587e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.407492e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 479:
Mobile Platform 1 Pose: [0.767349820442101;-9.97295580854084;-6.65558636843234]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60366092462372;-0.00246154026715742;-0.00729489637542037;-0.0024615402671759;-1.88193630550433;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252764897469;6.4227835643461]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08739834772446;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08810975026216;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.063858e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.061048e-200。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 480:
Mobile Platform 1 Pose: [0.767349820442101;-9.97179728409071;-6.66137162021828]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78902483664535;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06730021752596;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97438233214438;6.42581399855334]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79064904057539;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79136044311309;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.597993e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.524385e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 481:
Mobile Platform 1 Pose: [0.767349820442101;-9.97371949197019;-6.65149661595088]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48147157592857;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.75974695680918;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97327562463489;6.42382965635721]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.96772224209317;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96843364463087;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.531352e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.350450e-201。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 482:
Mobile Platform 1 Pose: [0.767349820442101;-9.9724278366542;-6.65833688483887]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.68813642648724;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.96641180736785;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97332883558977;6.42412519155109]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.95920848931173;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.95991989184943;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.117846e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.734635e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 483:
Mobile Platform 1 Pose: [0.767349820442101;-9.97218000278973;-6.65940953142868]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.72778984480214;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-2.00606522568275;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97435357144812;6.42559003676533]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79525075197599;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79596215451369;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.136031e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.199144e-202。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 484:
Mobile Platform 1 Pose: [0.767349820442101;-9.97377413400438;-6.65131305800422]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47272885045729;-0.00246154026715742;-0.00729489637542042;-0.00246154026717589;-1.7510042313379;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253121016471;6.42275048508477]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08682855732133;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08753995985903;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.554039e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.002743e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 485:
Mobile Platform 1 Pose: [0.767349820442101;-9.97196358037072;-6.66072647639522]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.7624174318437;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04069281272431;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97443981848357;6.42584602371336]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78145122630328;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78216262884098;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.099649e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.528884e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 486:
Mobile Platform 1 Pose: [0.767349820442101;-9.97269409158604;-6.65679014716975]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.64553563739207;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.92381101827268;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97315715152407;6.42359548815092]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.98667793982332;0.00253003695556727;0.00688343415761828;0.00252997841266994;1.98738934236102;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.085796e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.388283e-203。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 487:
Mobile Platform 1 Pose: [0.767349820442101;-9.9736106205806;-6.65226019823233]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49889099826246;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.77716637914307;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97345142857474;6.42427137854517]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9395936117161;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9403050142538;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.923121e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.758483e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 488:
Mobile Platform 1 Pose: [0.767349820442101;-9.97168917947428;-6.66212236960206]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.8063215752736;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08459695615421;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97428035368609;6.42540611664153]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80696559390049;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.80767699643819;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.176209e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.468315e-204。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 489:
Mobile Platform 1 Pose: [0.767349820442101;-9.9732010755179;-6.65422546676137]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56441820829403;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.84269358917464;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255088901358;6.42274231238517]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08367994150166;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08439134403935;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.541610e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.970904e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 490:
Mobile Platform 1 Pose: [0.767349820442101;-9.9732485915783;-6.65421456718185]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.55681563862992;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.83509101951053;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97448585355824;6.42585836287464]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77408561435649;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77479701689419;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.151524e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.862552e-205。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 491:
Mobile Platform 1 Pose: [0.767349820442101;-9.97167485562907;-6.66218020178715]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80861339050757;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.08688877138818;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97304397077477;6.42337179213159]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.00478685971191;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.00549826224961;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.763789e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.612108e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 492:
Mobile Platform 1 Pose: [0.767349820442101;-9.97358128357471;-6.65233279926288]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50358491920532;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78186030008593;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97357443799967;6.42441743872372]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91991210372866;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92062350626636;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.152353e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.076316e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 493:
Mobile Platform 1 Pose: [0.767349820442101;-9.97275008517691;-6.65683737583567]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.63657666285257;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.91485204373318;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97419690985185;6.42520707699244]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8203166073792;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.8210280099169;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.541924e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.011621e-206。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 494:
Mobile Platform 1 Pose: [0.767349820442101;-9.97192409500711;-6.66089423280056]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76873509002102;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04701047090163;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97258632937762;6.42275835575031]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07800948325634;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07872088579403;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.728210e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.694202e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 495:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737671358631;-6.65146222910365]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47384855306228;-0.00246154026715741;-0.00729489637542042;-0.00246154026717589;-1.75212393394289;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97452014608466;6.42585054826173]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76859881012932;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76931021266702;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.905206e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.408418e-207。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 496:
Mobile Platform 1 Pose: [0.767349820442101;-9.97223011835474;-6.6595322163865]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.71977135440061;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.99804673528123;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97293800622242;6.42316135682233]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.02174118808847;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.02245259062616;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.713477e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.570183e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 497:
Mobile Platform 1 Pose: [0.767349820442101;-9.97237365615747;-6.65859555844069]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.69680530596396;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.97508068684457;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97369593220519;6.42456029624846]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90047303084561;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90118443338331;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.331175e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.604143e-208。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 498:
Mobile Platform 1 Pose: [0.767349820442101;-9.97373556132622;-6.65172772699227]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47890047896354;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.75717585984416;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97410400503428;6.42499450987607]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83518137819054;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.83589278072824;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.337345e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.182034e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 499:
Mobile Platform 1 Pose: [0.767349820442101;-9.97182783819047;-6.66159414707407]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78413618068413;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06241156156474;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726368959217;6.42279752011239]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06991883620278;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07063023874048;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.001340e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.497601e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 500:
Mobile Platform 1 Pose: [0.767349820442101;-9.97290165579189;-6.65591173856142]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.61232536445639;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.89060074533701;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454249825079;6.42582230671325]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76502246354846;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76573386608616;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.842401e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.094425e-209。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 501:
Mobile Platform 1 Pose: [0.767349820442101;-9.97348992038532;-6.65309473146806]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.51820302950701;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.79647841038762;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9728410688176;6.4229667592484]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03725117285933;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03796257539703;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.050873e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.738238e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 502:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165092528039;-6.66248665723363]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.8124422462968;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.09071762717741;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97381409562898;6.42469703623237]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88156688303815;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.88227828557585;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.763706e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.289084e-210。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 503:
Mobile Platform 1 Pose: [0.767349820442101;-9.97337048459476;-6.65355076779672]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.53731275599778;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.81558813687839;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97400258484604;6.42477025641121]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.85140860830803;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.85212001084573;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.898046e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.252030e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 504:
Mobile Platform 1 Pose: [0.767349820442101;-9.97306388988018;-6.65536682387166]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.5863679103307;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.86464329121131;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97270169292785;6.42285833217064]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05955131521887;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06026271775656;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.301918e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.240697e-211。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 505:
Mobile Platform 1 Pose: [0.767349820442101;-9.97174331213415;-6.66199915449511]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79766034969549;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.0759357305761;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455279026936;6.42577354014405]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76337574057736;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76408714311506;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.839033e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.754901e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 506:
Mobile Platform 1 Pose: [0.767349820442101;-9.97367948960104;-6.65203303208749]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48787195499157;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.76614733587218;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9727548210912;6.42279032189809]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.05105080908263;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05176221162032;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.114491e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.594409e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 507:
Mobile Platform 1 Pose: [0.767349820442101;-9.97254078272283;-6.65810069534351]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.67006505550644;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.94834043638705;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97392726938067;6.42482496849794]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86345908276747;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86417048530516;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.464442e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.925503e-212。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 508:
Mobile Platform 1 Pose: [0.767349820442101;-9.97208246810444;-6.66025729415371]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.74339539444899;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.0216707753296;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97389378853181;6.42453641239021]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86881601858441;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86952742112211;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.556801e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.957894e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 509:
Mobile Platform 1 Pose: [0.767349820442101;-9.97378100500394;-6.65160445178623]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47162949052762;-0.00246154026715741;-0.00729489637542042;-0.00246154026717589;-1.74990487140823;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9727795863479;6.4229389705405]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.04708836801014;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04779977054784;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.693050e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.488442e-213。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 510:
Mobile Platform 1 Pose: [0.767349820442101;-9.97205097459753;-6.66062769189098]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.74843435555355;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.02670973643416;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455096960822;6.42570431157081]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76366704635959;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76437844889728;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.027546e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.231031e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 511:
Mobile Platform 1 Pose: [0.767349820442101;-9.97257955103044;-6.65772144852177]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.66386212628814;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.94213750716875;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97268074918723;6.42263408098919]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06290231371712;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06361371625481;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.041528e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.858758e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 512:
Mobile Platform 1 Pose: [0.767349820442101;-9.97366369910756;-6.65231376516615]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4903984339493;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.76867381482991;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97403397849322;6.42494167281657]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84638562475927;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.84709702729697;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.620189e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.167120e-214。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 513:
Mobile Platform 1 Pose: [0.767349820442101;-9.97172761046772;-6.66227760622036]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80017261632351;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07844799720412;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97377895458823;6.42429532506321]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88718944955821;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.88790085209591;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.003248e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.078000e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 514:
Mobile Platform 1 Pose: [0.767349820442101;-9.97309958572895;-6.65508569828562]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.58065657452735;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.85893195540796;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97286922639941;6.42303729591072]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.03274595976807;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03345736230576;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.182224e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.021221e-215。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 515:
Mobile Platform 1 Pose: [0.767349820442101;-9.97334086770348;-6.65407148436246]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.54205145860219;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.8203268394828;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453704583922;6.42561483808741]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.76589484939974;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76660625193743;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.826568e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.194072e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 516:
Mobile Platform 1 Pose: [0.767349820442101;-9.9716548940628;-6.66263463103049]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81180724110995;-0.00246154026715743;-0.00729489637542028;-0.0024615402671759;-2.09008262199056;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97262013919075;6.42249976040218]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07259991315523;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07331131569292;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.903638e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.373503e-216。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 517:
Mobile Platform 1 Pose: [0.767349820442101;-9.97351459543948;-6.65300946835671]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.5142550208425;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.79253040172312;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97413294538591;6.42504502493411]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83055092192943;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.83126232446713;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.033807e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.378454e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 518:
Mobile Platform 1 Pose: [0.767349820442101;-9.97286368008228;-6.65659285847588]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.61840147799319;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.8966768588738;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97365961599461;6.42404957701625]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90628362453739;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90699502707508;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.076799e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.476398e-217。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 519:
Mobile Platform 1 Pose: [0.767349820442101;-9.9718505347004;-6.66162109027367]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78050473909496;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.05878011997557;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97296907022906;6.42315088080583]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.01677094702522;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.01748234956291;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.220997e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.218351e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 520:
Mobile Platform 1 Pose: [0.767349820442101;-9.97374531406995;-6.65190332114231]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47734003996665;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75561542084726;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451109156626;6.4255054911856]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77004753307262;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77075893561032;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.161333e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.539982e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 521:
Mobile Platform 1 Pose: [0.767349820442101;-9.97233661102822;-6.65933199306478]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.70273252664364;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.98100790752425;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97257405465677;6.42238874617121]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07997343859041;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08068484112811;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.223494e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.046722e-218。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 522:
Mobile Platform 1 Pose: [0.767349820442101;-9.97226571428176;-6.65949529078849]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.71407600607681;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.99235138695743;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.974223091126;6.42513320566914]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81612760351477;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.81683900605247;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.537167e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.834319e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 523:
Mobile Platform 1 Pose: [0.767349820442101;-9.97376093034355;-6.65192331770552]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47484143619125;-0.00246154026715741;-0.00729489637542042;-0.00246154026717589;-1.75311681707186;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97353748260436;6.4238019534947]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.92582496697769;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92653636951539;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.114349e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.069200e-219。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 524:
Mobile Platform 1 Pose: [0.767349820442101;-9.97189811916186;-6.66158470482924]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77289122526081;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.05116660614142;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97307740658888;6.42327704222888]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.99943712945338;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.00014853199108;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.594916e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.248381e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 525:
Mobile Platform 1 Pose: [0.767349820442101;-9.97278884003199;-6.65683265784462]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.63037588603969;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.9086512669203;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97447324934671;6.42537680373795]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.77610228820077;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77681369073847;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.420407e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.615669e-220。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 526:
Mobile Platform 1 Pose: [0.767349820442101;-9.97355977836288;-6.65306335846176]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50702575309859;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.7853011339792;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254331406579;6.42230205940291]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08489193314845;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08560333568614;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.078140e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.233686e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 527:
Mobile Platform 1 Pose: [0.767349820442101;-9.97166676913853;-6.66275796827564]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80990722899453;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08818260987515;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97430352717483;6.4252046968429]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.803257835702;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.8039692382397;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.094422e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.268822e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 528:
Mobile Platform 1 Pose: [0.767349820442101;-9.973280688646;-6.65434915547652]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.55168010779854;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.82995548867915;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97341440964973;6.42355539159386]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.94551663971685;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.94622804225454;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.818450e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.806126e-221。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 529:
Mobile Platform 1 Pose: [0.767349820442101;-9.97316742818295;-6.65516884512172]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56980178188649;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.8480771627671;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97319238586442;6.4234128825722]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.98104044536649;0.00253003695556727;0.00688343415761828;0.00252997841266994;1.98175184790419;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.782570e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.922723e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 530:
Mobile Platform 1 Pose: [0.767349820442101;-9.97170091778318;-6.66256786986469]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80444344584984;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08271882673045;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97442374398601;6.42522948209801]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78402314591288;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78473454845058;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.010575e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.233538e-222。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 531:
Mobile Platform 1 Pose: [0.767349820442101;-9.97362972680792;-6.65262069772472]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49583400189209;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.77410938277271;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252846943112;6.42224032941258]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08726707469434;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08797847723204;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.803266e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.697774e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 532:
Mobile Platform 1 Pose: [0.767349820442101;-9.97265539380762;-6.65785425855523]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.65172728194048;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.93000266282109;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97437354051393;6.42525826789965]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79205570144594;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79276710398364;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.699216e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.274759e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 533:
Mobile Platform 1 Pose: [0.767349820442101;-9.97199229646895;-6.66106805803342]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.75782285612687;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.03609823700748;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9732923535915;6.42331291323046]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.96504560903435;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96575701157205;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.060165e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.562133e-223。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 534:
Mobile Platform 1 Pose: [0.767349820442101;-9.97377770413127;-6.65194877422797]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47215763015574;-0.00246154026715741;-0.00729489637542042;-0.00246154026717589;-1.75043301103635;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97331205838987;6.42355534356638]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.96189284129512;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96260424383282;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.297417e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.014141e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 535:
Mobile Platform 1 Pose: [0.767349820442101;-9.97214637167306;-6.66048678707421]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.73317082346886;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.01144620434947;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97436289913854;6.42506442135134]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.79375832150825;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79446972404594;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.033386e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.434385e-224。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 536:
Mobile Platform 1 Pose: [0.767349820442101;-9.97246589200801;-6.65864905380971]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.68204756987809;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.96032295075871;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252978921358;6.42220377183699]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08705590950105;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08776731203874;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.642193e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.652188e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 537:
Mobile Platform 1 Pose: [0.767349820442101;-9.97370721252974;-6.65241611782451]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48343628640117;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76171166728178;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97443257473905;6.4252929564478]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.78261022542646;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78332162796416;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.682185e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.626351e-225。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 538:
Mobile Platform 1 Pose: [0.767349820442101;-9.97177792308811;-6.66237159699903]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.7921225970621;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07039797794271;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97317331910686;6.42307754767678]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.98409112657691;0.00253003695556727;0.00688343415761828;0.00252997841266994;1.9848025291146;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.472547e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.157578e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 539:
Mobile Platform 1 Pose: [0.767349820442101;-9.9729929094869;-6.65597349086554]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.5977247732547;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.87600015413531;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97343442183311;6.42370127476041]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9423146903764;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.94302609291409;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.085820e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.383029e-226。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 540:
Mobile Platform 1 Pose: [0.767349820442101;-9.9734259112452;-6.6539641429967]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.52844449192804;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.80671987280865;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97429115683528;6.42488272181034]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.80523709002934;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80594849256703;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.795247e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.534201e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 541:
Mobile Platform 1 Pose: [0.767349820442101;-9.97164755056477;-6.66302465220332]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81298220079573;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.09125758167635;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254724909828;6.42219217709602]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08426232794914;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08497373048684;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.295664e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.972913e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 542:
Mobile Platform 1 Pose: [0.767349820442101;-9.97343911564002;-6.65373239784221]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.52633178875615;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.80460716963676;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97448020928189;6.42530804527882]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77498869857164;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77570010110933;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.769749e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.193518e-227。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 543:
Mobile Platform 1 Pose: [0.767349820442101;-9.97297496506163;-6.65635836535617]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60059588129915;-0.00246154026715742;-0.00729489637542036;-0.00246154026717589;-1.87887126217976;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97305930290345;6.42285225214938]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.0023337191216;0.00253003695556727;0.00688343415761828;0.00252997841266994;2.0030451216593;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.887796e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.645474e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 544:
Mobile Platform 1 Pose: [0.767349820442101;-9.97178728285223;-6.66229574860146]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79062503480347;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06890041568408;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97355747539277;6.42384751338337]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.92262612083158;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92333752336927;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.851524e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.941976e-228。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 545:
Mobile Platform 1 Pose: [0.767349820442101;-9.97371342289479;-6.65239716052286]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48244262799273;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76071800887334;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97420909837517;6.42468570519885]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81836644364747;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.81907784618517;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.807916e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.541133e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 546:
Mobile Platform 1 Pose: [0.767349820442101;-9.97244741726782;-6.65910813855249]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.6850035283087;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.96327890918931;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97258053285084;6.42220491256682]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07893692754045;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07964833007815;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.276707e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.761909e-229。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 547:
Mobile Platform 1 Pose: [0.767349820442101;-9.97216274447688;-6.66037031612281]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.73055117485849;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.0088265557391;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451613860349;6.42530303808846]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76924000711585;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76995140965354;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.821796e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.992050e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 548:
Mobile Platform 1 Pose: [0.767349820442101;-9.97377621864209;-6.6521710180591]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47239530842541;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75067068930602;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97295224039963;6.42263983919234]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.01946371973275;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.02017512227045;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.555737e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.631685e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 549:
Mobile Platform 1 Pose: [0.767349820442101;-9.97197831525384;-6.66152364132518]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.7600598505447;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.03833523142531;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9736792758928;6.42399096836354]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90313804082622;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90384944336391;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.941839e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.635876e-230。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 550:
Mobile Platform 1 Pose: [0.767349820442101;-9.97267431280371;-6.65776321728874]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.64870024256523;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.92697562344584;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97411746488806;6.42447492926307]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83302780158401;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.83373920412171;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.887519e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.624943e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 551:
Mobile Platform 1 Pose: [0.767349820442101;-9.97362070235295;-6.65307707248538]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49727791468688;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.77555329556749;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726290430431;6.4222409382108]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07117529677746;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07188669931515;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.185836e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.295960e-231。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 552:
Mobile Platform 1 Pose: [0.767349820442101;-9.97169516100113;-6.6629648827995]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80536453097811;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08363991185873;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454015302789;6.42527763612325]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76539769921148;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76610910174918;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.576988e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.960508e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 553:
Mobile Platform 1 Pose: [0.767349820442101;-9.97318395190964;-6.65518418947529]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56715798561686;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.84543336649747;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97285395982251;6.42244291703803]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.03518861207193;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03590001460963;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.523725e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.425438e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 554:
Mobile Platform 1 Pose: [0.767349820442101;-9.97326534477375;-6.65499806264019]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.55413512735978;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.83241050824039;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97379798966592;6.42412869941771]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88414383712664;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.88485523966434;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.460236e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.430750e-232。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 555:
Mobile Platform 1 Pose: [0.767349820442101;-9.9716706255736;-6.66307499920554]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80929019938294;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08756558026355;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97401717568548;6.42425219940935]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84907407399724;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.84978547653494;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.670483e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.368281e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 556:
Mobile Platform 1 Pose: [0.767349820442101;-9.9735704421151;-6.65325840316905]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50531955274453;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78359493362514;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97269191911641;6.42229883195653]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.06111512504863;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06182652758633;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.224769e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.462958e-233。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 557:
Mobile Platform 1 Pose: [0.767349820442101;-9.97277022152926;-6.65760492697638]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.63335484647781;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.91163022735842;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455212275313;6.42523171803556]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76348254317309;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76419394571078;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.165934e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.544934e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 558:
Mobile Platform 1 Pose: [0.767349820442101;-9.97191067092919;-6.66183573279131]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77088294248918;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04915832336979;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97276614546541;6.4222638446876]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.04923890920848;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04995031174618;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.092374e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.675768e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 559:
Mobile Platform 1 Pose: [0.767349820442101;-9.97376420623704;-6.65234630769216]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47431729323348;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.7525926741141;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97391193476878;6.42425798306704]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86591262066827;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86662402320597;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.020434e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.274658e-234。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 560:
Mobile Platform 1 Pose: [0.767349820442101;-9.97224856815794;-6.66030976807394]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.71681938588976;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.99509476677038;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97390934217605;6.42401957524826]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86632743550626;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86703883804396;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.680723e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.527622e-235。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 561:
Mobile Platform 1 Pose: [0.767349820442101;-9.97235458372647;-6.65956541697018]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.69985689492424;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.97813227580485;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97276805917203;6.42237681959039]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.04893271614812;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04964411868582;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.915301e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.672225e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 562:
Mobile Platform 1 Pose: [0.767349820442101;-9.97374089965374;-6.65256884753091]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47804634656154;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75632172744215;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.974551986374;6.42516532504428]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76350436383361;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76421576637131;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.016693e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.558481e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 563:
Mobile Platform 1 Pose: [0.767349820442101;-9.97183958320751;-6.66240692231377]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78225697795762;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06053235883823;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97269030886678;6.42210469894545]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.06137276498887;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06208416752657;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.261066e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.195417e-236。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 564:
Mobile Platform 1 Pose: [0.767349820442101;-9.9728821698863;-6.65688304551479]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.61544310935132;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.89371849023193;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97401961014744;6.42437636054003]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84868456008321;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.84939596262091;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.333867e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.676309e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 565:
Mobile Platform 1 Pose: [0.767349820442101;-9.97350299481287;-6.65389646945466]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.5161111211005;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.79438650198111;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97379527469529;6.423779368843]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88457823242804;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.88528963496574;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.297942e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.956903e-237。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 566:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165295297752;-6.66334949701347]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81211781475683;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.09039319563744;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97285614256478;6.42247280500712]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.03483937330917;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03555077584686;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.917376e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.607732e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 567:
Mobile Platform 1 Pose: [0.767349820442101;-9.97335536899228;-6.65449886151911]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.53973125239508;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.81800663327569;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453974491492;6.42507865290163]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.7654629972866;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7661743998243;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.781381e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.883711e-238。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 568:
Mobile Platform 1 Pose: [0.767349820442101;-9.97308266008345;-6.65614048772896]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.58336467780743;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.86164005868804;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97262776476801;6.42196724793331]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07137982079298;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07209122333068;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.520249e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.795852e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 569:
Mobile Platform 1 Pose: [0.767349820442101;-9.9717351337929;-6.66291408040988]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79896888429601;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07724426517662;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97411971094722;6.42448166636322]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83266843211836;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.83337983465606;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.373700e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.826891e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 570:
Mobile Platform 1 Pose: [0.767349820442101;-9.97367157256087;-6.65294324534464]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48913868142049;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.7674140623011;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97367647934899;6.42353413063593]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90358548783635;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90429689037404;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.936591e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.566707e-239。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 571:
Mobile Platform 1 Pose: [0.767349820442101;-9.97256110247951;-6.65886788525712]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.66681389443849;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.9450892753191;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97295465247462;6.42258439988558]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.01907778773487;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.01978919027256;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.706597e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.651905e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 572:
Mobile Platform 1 Pose: [0.767349820442101;-9.97206611802013;-6.6612135901895]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.74601140793938;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.02428678881999;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451546190553;6.42497205121086]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76934827878932;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77005968132702;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.748199e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.450968e-240。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 573:
Mobile Platform 1 Pose: [0.767349820442101;-9.97378132491742;-6.65247155381558]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47157830437163;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.74985368525224;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97257960862079;6.42185292573443]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.07908480434854;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.07979620688624;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.482651e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.674350e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 574:
Mobile Platform 1 Pose: [0.767349820442101;-9.97206746359587;-6.66141572291941]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.74579611582063;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.02407149670124;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9742111313214;6.42457203959587]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.81804117225026;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.81875257478796;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.872699e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.147883e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 575:
Mobile Platform 1 Pose: [0.767349820442101;-9.97255947528905;-6.65869621042693]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.66707424491076;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.94534962579137;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97355464230628;6.42328661925441]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.92307941466871;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92379081720641;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.325286e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.301829e-241。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 576:
Mobile Platform 1 Pose: [0.767349820442101;-9.97367227771463;-6.65313825006741]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48902585681848;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76730123769909;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97306190010055;6.42270895560742]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.00191816758669;0.00253003695556727;0.00688343415761828;0.00252997841266994;2.00262957012439;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.868082e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.369334e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 577:
Mobile Platform 1 Pose: [0.767349820442101;-9.97173583529353;-6.66310861937969]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.7988566441937;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07713202507431;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97447926949024;6.42484602955591]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77513906523521;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77585046777291;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.221417e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.386678e-242。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 578:
Mobile Platform 1 Pose: [0.767349820442101;-9.9730811638208;-6.65605126214005]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.58360407983188;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.86187946071249;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254669404953;6.42176280552752]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08435113574876;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08506253828646;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.122231e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.226626e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 579:
Mobile Platform 1 Pose: [0.767349820442101;-9.97335668068735;-6.6548595310922]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.53952138118376;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.81779676206437;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97429295732894;6.42464592135263]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.80494901104405;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80566041358175;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.145344e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.119652e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 580:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165281800359;-6.66351849056636]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81213941058436;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.09041479146497;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97343160024113;6.42303975323137]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.94276614509287;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.94347754763057;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.586480e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.128915e-243。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 581:
Mobile Platform 1 Pose: [0.767349820442101;-9.9735019645319;-6.65394445318481]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.51627596605605;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.79455134693667;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9731760537175;6.42284360265737]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.98365358887405;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.98436499141174;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.913152e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.508706e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 582:
Mobile Platform 1 Pose: [0.767349820442101;-9.97288384781389;-6.65735993092569]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.61517464093733;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.89345002181794;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97443138002264;6.42470126899589]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.78280138005169;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78351278258939;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.962807e-246。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.465739e-244。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 583:
Mobile Platform 1 Pose: [0.767349820442101;-9.97183864946543;-6.66255485999852]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78240637669093;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06068175757154;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252961115214;6.42169757246899]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08708439933194;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08779580186964;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.590895e-246。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.231167e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 584:
Mobile Platform 1 Pose: [0.767349820442101;-9.97374052851684;-6.65279708569982]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47810572846453;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75638110934514;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97436445282644;6.42470204251724]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.79350973144324;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79422113398094;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.180967e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.469371e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 585:
Mobile Platform 1 Pose: [0.767349820442101;-9.97235624957368;-6.66010321953454]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.69959035936997;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.97786574025058;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97330929779962;6.42279654601272]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.96233453573459;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96304593827229;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.733893e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.022448e-245。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 586:
Mobile Platform 1 Pose: [0.767349820442101;-9.97224709479436;-6.66046299719209]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.71705512406113;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.99533050494174;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97329517570146;6.42298530253428]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.96459407144019;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96530547397789;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.550492e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.920176e-246。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 587:
Mobile Platform 1 Pose: [0.767349820442101;-9.97376456453744;-6.65277320194294]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47425996516939;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75253534605;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97437210213317;6.42453863698766]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.79228584236751;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.79299724490521;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.320062e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.365393e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 588:
Mobile Platform 1 Pose: [0.767349820442101;-9.97191190151611;-6.66238695118784]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77068604858149;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.0489614294621;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252866859995;6.42165750078579]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08723520768276;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08794661022046;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.632467e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.551502e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 589:
Mobile Platform 1 Pose: [0.767349820442101;-9.97276860986779;-6.6578079547903]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.63361271231334;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.91188809319395;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97442504099762;6.4247394049777]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.78381562405559;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78452702659329;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.136755e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.046701e-247。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 590:
Mobile Platform 1 Pose: [0.767349820442101;-9.97357150856016;-6.65387181343686]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.5051489215338;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78342430241441;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97318973554364;6.42256002952315]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.98146449669128;0.00253003695556726;0.00688343415761828;0.00252997841266994;1.98217589922898;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.117693e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.049373e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 591:
Mobile Platform 1 Pose: [0.767349820442101;-9.97167109161665;-6.66360909050454]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80921563249494;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08749101337555;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97341726868473;6.42313091424279]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.94505919411741;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.94577059665511;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.734446e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.511976e-248。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 592:
Mobile Platform 1 Pose: [0.767349820442101;-9.97326400604203;-6.65530548031839]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.55434932443499;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.8326247053156;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9743018599268;6.42435920379865]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.80352459538607;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80423599792377;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.217039e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.196646e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 593:
Mobile Platform 1 Pose: [0.767349820442101;-9.97318558414747;-6.65594534891074]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56689682756348;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.84517220844409;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254388314328;6.42164244052786]
Robot Arm 2 Joint Angles: [0.00253002436772006;-2.08480088075038;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.08551228328808;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.108919e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.105323e-249。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 594:
Mobile Platform 1 Pose: [0.767349820442101;-9.97169473624772;-6.6634726919914]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80543249152404;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08370787240465;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97447428373375;6.42475725901523]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.7759367862747;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7766481888124;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.821001e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.653466e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 595:
Mobile Platform 1 Pose: [0.767349820442101;-9.97361993948592;-6.65354070556297]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4973999734115;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.77567535429211;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97307491386689;6.42233317452373]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.99983596497265;0.00253003695556727;0.00688343415761828;0.00252997841266994;2.00054736751035;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.753230e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.705349e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 596:
Mobile Platform 1 Pose: [0.767349820442101;-9.9726762037569;-6.65861863535895]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.64839769005504;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.92667307093565;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9735403290158;6.42327727287719]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.92536954114679;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.92608094368449;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.911425e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.226767e-250。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 597:
Mobile Platform 1 Pose: [0.767349820442101;-9.97197711860065;-6.66201851261492]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76025131505535;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.03852669593596;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97422121376888;6.42416425880266]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.81642798065412;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.81713938319182;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.557731e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.233173e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 598:
Mobile Platform 1 Pose: [0.767349820442101;-9.97377619034822;-6.65282540616609]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47239983544368;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75067521632429;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9725749790128;6.42165181775328]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07982554162711;0.00253003695556727;0.00688343415761825;0.00252997841266995;2.0805369441648;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.970332e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.489059e-251。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 599:
Mobile Platform 1 Pose: [0.767349820442101;-9.97216445731791;-6.66126645520136]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.73027712029343;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.00855250117404;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451186074164;6.42475507909151]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76992446501219;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77063586754989;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.335459e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.744952e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 600:
Mobile Platform 1 Pose: [0.767349820442101;-9.97244578587327;-6.65962414312023]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.68526455143611;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.96353993231672;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97296677898188;6.42211881663401]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.01713754657369;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.01784894911139;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.013119e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.370394e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 601:
Mobile Platform 1 Pose: [0.767349820442101;-9.97371417791618;-6.65324882142943]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4823218245713;-0.00246154026715742;-0.00729489637542041;-0.00246154026717589;-1.76059720545191;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97366240291478;6.42342127354301]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90583771730966;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90654911984736;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.826187e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.028392e-252。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 602:
Mobile Platform 1 Pose: [0.767349820442101;-9.97178837857804;-6.66319114515288]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79044971867345;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06872509955406;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97413088098769;6.42395532537467]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.83088122564402;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.83159262818172;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.541482e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.980728e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 603:
Mobile Platform 1 Pose: [0.767349820442101;-9.97297334990583;-6.65694507051656]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60085430622654;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.87912968710716;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97262139736924;6.42168464842568]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07239860459606;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07311000713376;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.573783e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.509059e-253。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 604:
Mobile Platform 1 Pose: [0.767349820442101;-9.97344060081712;-6.65475770523242]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.52609416041984;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.80436954130045;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745375500649;6.42473254024378]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76581417329051;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.7665255758282;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.024978e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.429914e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 605:
Mobile Platform 1 Pose: [0.767349820442101;-9.9716477377085;-6.66389699626873]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.8129522577992;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.09122763867982;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97286717583374;6.42191959470096]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.0330740502759;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.0337854528136;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.910154e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.575214e-254。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 606:
Mobile Platform 1 Pose: [0.767349820442101;-9.97342472783213;-6.65467659838674]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.52863383801903;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.80690921889964;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97378163927569;6.42355995162711]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88675989956487;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.88747130210257;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.756656e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.944157e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 607:
Mobile Platform 1 Pose: [0.767349820442101;-9.97299491378906;-6.65712623403422]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.59740408490961;-0.00246154026715742;-0.00729489637542036;-0.00246154026717589;-1.87567946579022;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97403175464669;6.42373417304125]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.84674144020386;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.84745284274156;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.165547e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.161889e-255。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 608:
Mobile Platform 1 Pose: [0.767349820442101;-9.97177715215357;-6.66322067714735]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.7922459465887;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.07052132746932;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97268231354202;6.42173956272774]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.06265201695152;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06336341948922;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.088246e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.562152e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 609:
Mobile Platform 1 Pose: [0.767349820442101;-9.97370673622316;-6.65330086315023]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48351249545397;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76178787633458;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455121157461;6.42468949736933]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76362833173655;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76433973427425;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.743862e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.514369e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 610:
Mobile Platform 1 Pose: [0.767349820442101;-9.97246801601374;-6.65987417870795]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.68170772895988;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.9599831098405;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97277781011368;6.42173790392607]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.04737256548531;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.04808396802301;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.706431e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.083378e-256。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 611:
Mobile Platform 1 Pose: [0.767349820442101;-9.97214486015598;-6.66133442593957]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.73341266620164;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.01168804708225;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97389633346897;6.42369055099087]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86840882863974;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86912023117744;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.386598e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.064515e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 612:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737780661546;-6.6530301306835]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4720997064238;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75037508730441;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97392491822171;6.42350282490052]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.86383526820057;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.86454667073827;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.490436e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.615467e-257。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 613:
Mobile Platform 1 Pose: [0.767349820442101;-9.97199403223622;-6.66231585000261]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.75754513336343;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.03582051424404;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97275665849628;6.4218148346073]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.05075682426987;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.05146822680757;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.398006e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.291326e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 614:
Mobile Platform 1 Pose: [0.767349820442101;-9.97265357547758;-6.65874128931413]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.65201821474575;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.93029359562636;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455277479846;6.4246259695442]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76337821592077;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76408961845847;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.156442e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.875829e-258。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 615:
Mobile Platform 1 Pose: [0.767349820442101;-9.97363095221955;-6.65389298230932]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49563793603039;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.77391331691101;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9727002185125;6.42157586154611]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.0597872216741;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.0604986242118;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.329918e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.694499e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 616:
Mobile Platform 1 Pose: [0.767349820442101;-9.97170181660947;-6.66380406533225]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80429963364366;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08257501452427;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97400495842191;6.42381057453661]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.851028836169;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.8517402387067;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.593579e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.055305e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 617:
Mobile Platform 1 Pose: [0.767349820442101;-9.97316581130901;-6.65614818437729]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.57006048171798;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.84833586259859;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97381165359259;6.42326355726577]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.88195760886079;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.88266901139849;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.143920e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.245116e-259。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 618:
Mobile Platform 1 Pose: [0.767349820442101;-9.97328266983238;-6.65577852955746]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.55136311797766;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.82963849885827;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97284314140781;6.4219084121036]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.03691955842476;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03763096096246;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.789207e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.279240e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 619:
Mobile Platform 1 Pose: [0.767349820442101;-9.97166659357327;-6.66396892173796]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80993531943587;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08821070031648;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454223214148;6.42454213098394]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76506504103709;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76577644357479;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.010980e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.437096e-260。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 620:
Mobile Platform 1 Pose: [0.767349820442101;-9.9735587797729;-6.65418823725091]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.5071855274959;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.78546090837651;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97263574423919;6.42143527981568]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07010310540408;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07081450794178;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.543070e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.637315e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 621:
Mobile Platform 1 Pose: [0.767349820442101;-9.97279126273985;-6.65836779755885]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.62998825278264;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.90826363366325;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97410618181861;6.42391781542532]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.83483309269608;0.00253003695556726;0.00688343415761833;0.00252997841266993;1.83554449523378;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.326806e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.161184e-261。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 622:
Mobile Platform 1 Pose: [0.767349820442101;-9.97189691852244;-6.66277904641729]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77308332756952;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.05135870845013;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97369343946039;6.42301888757986]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90087187001144;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90158327254914;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.929946e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.919871e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 623:
Mobile Platform 1 Pose: [0.767349820442101;-9.97376079716295;-6.65323281542502]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47486274508792;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75313812596853;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9729402721764;6.42201794699375]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.02137863545065;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.02209003798835;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.863430e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.000374e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 624:
Mobile Platform 1 Pose: [0.767349820442101;-9.97226806026779;-6.66108201184313]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.71370064831236;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.99197602919297;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97451963810213;6.4244383093394]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76868008733401;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.76939148987171;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.043964e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.151713e-262。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 625:
Mobile Platform 1 Pose: [0.767349820442101;-9.97233472937378;-6.6605393896946]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.70303359135514;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.98130897223575;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97258551447417;6.42131764075263]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07813986780795;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07885127034564;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.426565e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.259136e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 626:
Mobile Platform 1 Pose: [0.767349820442101;-9.97374615222484;-6.65341032548346]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47720593518504;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75548131606565;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97419887052039;6.42401037053868]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.82000290041209;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.82071430294979;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.220182e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.051756e-263。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 627:
Mobile Platform 1 Pose: [0.767349820442101;-9.97185222388089;-6.66321520608372]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78023447021662;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.05850985109723;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97357193753049;6.42277154668223]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.92031217879602;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.92102358133372;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.926264e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.197497e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 628:
Mobile Platform 1 Pose: [0.767349820442101;-9.97286166614647;-6.65785964760467]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.61872370772444;-0.00246154026715742;-0.00729489637542036;-0.0024615402671759;-1.89699908860506;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97304638520014;6.42214082608905]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.00440055165247;0.00253003695556727;0.00688343415761828;0.00252997841266994;2.00511195419017;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.419515e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.164674e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 629:
Mobile Platform 1 Pose: [0.767349820442101;-9.9735163930346;-6.65469644012149]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.51396740562334;-0.00246154026715742;-0.0072948963754204;-0.00246154026717589;-1.79224278650395;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97448511455252;6.42431499093703]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77420385527069;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77491525780839;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.244482e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.577628e-264。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 630:
Mobile Platform 1 Pose: [0.767349820442101;-9.9716554886225;-6.6642098673966]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81171211155955;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08998749244017;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255041785696;6.42122406951506]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08375532656104;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08446672909874;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.789848e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.320325e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 631:
Mobile Platform 1 Pose: [0.767349820442101;-9.97333928377323;-6.65545197226767]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.54230488744339;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.820580268324;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97428208468223;6.42408663968514]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.80668863451742;0.00253003695556725;0.00688343415761834;0.00252997841266993;1.80740003705512;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.097388e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.375690e-265。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 632:
Mobile Platform 1 Pose: [0.767349820442101;-9.97310214448933;-6.65691040903027]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.58024717286664;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.85852255374725;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9734489649228;6.42252443310602]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93998779602573;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.94069919856343;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.804622e-267。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.063090e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 633:
Mobile Platform 1 Pose: [0.767349820442101;-9.97172695399727;-6.66382915315411]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80027765159633;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.07855303247694;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97315966751757;6.42227420921643]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.98627538086378;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.98698678340148;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.421385e-267。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.605559e-266。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 634:
Mobile Platform 1 Pose: [0.767349820442101;-9.97366295189106;-6.65385707014918]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49051798859033;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76879336947094;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97443886160698;6.42417283162406]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.78160432655785;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78231572909555;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.328051e-268。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.630318e-267。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 635:
Mobile Platform 1 Pose: [0.767349820442101;-9.97258241225048;-6.65962998677364]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.6634043310824;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.94167971196301;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253108253856;6.42115530711939]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08684897750443;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08756038004213;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.643318e-268。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.675025e-267。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 636:
Mobile Platform 1 Pose: [0.767349820442101;-9.97204924946564;-6.66217266638997]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.74871037665737;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.02698575753799;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97435506446869;6.42414531448083]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.79501186868353;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.79572327122123;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.121978e-269。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.499037e-268。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 637:
Mobile Platform 1 Pose: [0.767349820442101;-9.97378131051253;-6.65334031937234]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47158060915479;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.74985599003541;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97332645333865;6.42228055024301]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9595896494897;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9603010520274;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.133973e-269。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.356366e-268。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 638:
Mobile Platform 1 Pose: [0.767349820442101;-9.97208497659903;-6.66219856753208]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.74299403531427;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.02126941619488;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9732781945569;6.42241507911831]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.96731105457044;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.96802245710814;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.148389e-269。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.224120e-268。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 639:
Mobile Platform 1 Pose: [0.767349820442101;-9.97253849313543;-6.65967571411127]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.67043138948996;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.94870677037057;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97438117312573;6.42401267132158]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.79083448355672;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.79154588609442;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.661694e-270。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.121348e-269。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 640:
Mobile Platform 1 Pose: [0.767349820442101;-9.97368092454575;-6.65396228779057]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48764236383933;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76591774471994;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252785753424;6.42111168661664]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08736497819659;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08807638073429;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.718892e-270。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.233387e-269。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 641:
Mobile Platform 1 Pose: [0.767349820442101;-9.97174483320574;-6.66393578655069]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79741697824168;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.07569235912229;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97441721206548;6.42418536033548]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.78506825319816;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78577965573585;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.867381e-271。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.741517e-270。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 642:
Mobile Platform 1 Pose: [0.767349820442101;-9.97306170739253;-6.65702219578855]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.58671710835519;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.8649924892358;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97320639807845;6.42204293111552]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.97879849112234;0.00253003695556726;0.00688343415761829;0.00252997841266994;1.97950989366004;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.883231e-271。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.002124e-270。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 643:
Mobile Platform 1 Pose: [0.767349820442101;-9.97337297207112;-6.65564502102319]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.53691475978038;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.81519014066099;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97339997501519;6.42256030589018]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.94782618124413;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.94853758378183;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.203395e-272。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.599834e-271。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 644:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165106576373;-6.66440082987727]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81241976896315;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.09069514984376;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97431245554965;6.42383555032319]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.80182929573101;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.80254069826871;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.182038e-272。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.031314e-271。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 645:
Mobile Platform 1 Pose: [0.767349820442101;-9.97348844371147;-6.6548840325055]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.51843929732403;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.79671467820464;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97254080099837;6.42109311819913]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08529402393512;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08600542647282;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.210514e-272。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.940868e-271。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 646:
Mobile Platform 1 Pose: [0.767349820442101;-9.97290485730627;-6.65812262910211]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.61181312215554;-0.00246154026715742;-0.00729489637542036;-0.00246154026717589;-1.89008850303615;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97446807125658;6.42420599425789]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77693078262213;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.77764218515983;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.055329e-273。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.911361e-272。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 647:
Mobile Platform 1 Pose: [0.767349820442101;-9.97182656067983;-6.66348980826817]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78434058238652;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06261596326714;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97309080218379;6.42181455868052]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.99729383426829;0.00253003695556727;0.00688343415761828;0.00252997841266994;1.99800523680599;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.593917e-273。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.435079e-272。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 648:
Mobile Platform 1 Pose: [0.767349820442101;-9.97373517120517;-6.65369376594632]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47896289833152;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75723827921214;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97352300348181;6.42270672413943]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.92814162658414;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.92885302912184;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.449672e-274。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.755133e-273。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 649:
Mobile Platform 1 Pose: [0.767349820442101;-9.97237692823221;-6.66086913083253]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.69628177400649;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.9745571548871;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97423324854685;6.42364272567838]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.81450241617747;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.81521381871517;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.242819e-274。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.741986e-273。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 650:
Mobile Platform 1 Pose: [0.767349820442101;-9.97222778389481;-6.66143436067019]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.72014486798927;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-1.99842024886988;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97256967816218;6.42109908738576]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08067367772504;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08138508026274;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.948243e-275。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.649455e-273。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 651:
Mobile Platform 1 Pose: [0.767349820442101;-9.97376801393476;-6.65362396223675]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47370806159851;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75198344247912;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97450730647922;6.42420666075316]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77065314699881;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.77136454953651;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.626986e-275。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.672990e-274。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 652:
Mobile Platform 1 Pose: [0.767349820442101;-9.97192664668362;-6.66318434915986]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76832682178063;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04660220266124;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97298162182372;6.42159829061477]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.01476269187918;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.01547409441688;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.030387e-275。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.548889e-274。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 653:
Mobile Platform 1 Pose: [0.767349820442101;-9.97274738716906;-6.65878842437632]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.63700834410912;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.91528372498973;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97364531653258;6.42285121662634]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.90857153846137;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.90928294099907;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.138432e-276。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.469824e-275。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 654:
Mobile Platform 1 Pose: [0.767349820442101;-9.97358346298745;-6.65467901215911]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50323621316687;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.78151159404748;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97414424581994;6.42343568634807]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.82874285248318;0.00253003695556726;0.00688343415761834;0.00252997841266994;1.82945425502088;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.918456e-276。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.922660e-275。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 655:
Mobile Platform 1 Pose: [0.767349820442101;-9.97167603517552;-6.66445715847964]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80842466307655;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08670004395716;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97261396945023;6.42112866710737]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.0735870716376;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.0742984741753;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.152341e-277。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.931526e-276。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 656:
Mobile Platform 1 Pose: [0.767349820442101;-9.97324631310397;-6.6562670394576]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.55718019452444;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.83545557540506;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453468306119;6.42418700800999]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76627289388344;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76698429642114;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.614728e-277。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.821325e-276。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 657:
Mobile Platform 1 Pose: [0.767349820442101;-9.9732043683257;-6.65671853899163]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56389135904697;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.84216673992758;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97288071805776;6.42139679572337]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.03090729443312;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03161869697082;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.915886e-278。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.024063e-276。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 658:
Mobile Platform 1 Pose: [0.767349820442101;-9.97168868243961;-6.66437700037775]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80640110082115;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08467648170176;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97376504634412;6.42299079555408]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.8894147686147;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.8901261711524;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.905010e-278。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.595929e-277。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 659:
Mobile Platform 1 Pose: [0.767349820442101;-9.97360936739249;-6.65446475274913]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49909150836104;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.77736688924165;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97404631426027;6.42321616583294]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.8444119020317;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.8451233045694;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.099547e-278。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.388699e-277。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 660:
Mobile Platform 1 Pose: [0.767349820442101;-9.97269796049105;-6.65937812651432]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.64491661259212;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.92319199347274;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97267288681418;6.42118054083497]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.06416029340475;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06487169594245;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.608844e-279。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.257044e-278。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 661:
Mobile Platform 1 Pose: [0.767349820442101;-9.97196153890802;-6.66297115521596]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76274406587606;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04101944675667;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455005021439;6.42414686665336]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76381414937182;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76452555190952;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.616654e-279。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.228988e-278。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 662:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737742460643;-6.65370420812292]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47271092087069;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.7509863017513;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97278981757634;6.42121250505072]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.04545137146017;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.04616277399787;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.905277e-280。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.010464e-279。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 663:
Mobile Platform 1 Pose: [0.767349820442101;-9.97218359235463;-6.66204078987566]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.72721551441875;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.00549089529936;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97388046602204;6.4231226728457]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.87094762014835;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.87165902268605;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.679358e-280。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.019073e-279。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 664:
Mobile Platform 1 Pose: [0.767349820442101;-9.97242484168785;-6.66060363084359]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.68861562110327;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.96689100198388;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9739405093413;6.42298615041521]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.86134068906706;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.86205209160477;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.263051e-281。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.033301e-279。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 665:
Mobile Platform 1 Pose: [0.767349820442101;-9.97372111122741;-6.6540815998859]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.481212494773;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75948787565362;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97274539480952;6.42125303168657]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.05255901415163;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.05327041668933;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.510151e-281。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.096295e-280。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 666:
Mobile Platform 1 Pose: [0.767349820442101;-9.9717996959364;-6.66400638250035]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78863894133456;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06691432221517;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455332818591;6.42408623324694]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76328967392851;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76400107646621;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.636284e-282。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.815646e-280。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 667:
Mobile Platform 1 Pose: [0.767349820442101;-9.97295275398447;-6.65792203674008]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60414965364449;-0.00246154026715742;-0.00729489637542036;-0.00246154026717589;-1.88242503452511;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97271048195898;6.42104757608713]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.0581450702382;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.0588564727759;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.985038e-282。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.956946e-281。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 668:
Mobile Platform 1 Pose: [0.767349820442101;-9.97345567138631;-6.65554920560619]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.52368286935018;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.80195825023079;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97399002258947;6.42324431336257]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.85341856935955;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.85412997189725;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.914702e-282。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.333654e-281。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 669:
Mobile Platform 1 Pose: [0.767349820442101;-9.97164837157801;-6.66476718786663]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81285083867778;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.09112621955839;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9738280842229;6.42274788012827]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.87932870800962;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.88004011054732;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.837365e-283。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.928558e-282。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 670:
Mobile Platform 1 Pose: [0.767349820442101;-9.97340939015701;-6.65562571046213]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.53108786603842;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.80936324691904;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97283023312785;6.42134413279993]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.03898488321803;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.03969628575573;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.918028e-283。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.773781e-282。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 671:
Mobile Platform 1 Pose: [0.767349820442101;-9.97301563023124;-6.65789009233827]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.5940894541612;-0.00246154026715742;-0.00729489637542036;-0.00246154026717589;-1.87236483504181;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454450067232;6.4240052602539]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76470207610381;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76541347864151;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.211728e-284。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.530574e-282。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 672:
Mobile Platform 1 Pose: [0.767349820442101;-9.97176694662404;-6.66414612798879]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79387883131265;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.07215421219326;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97264408268074;6.42090386510067]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.06876895475633;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.06948035729403;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.772370e-284。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.615929e-283。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 673:
Mobile Platform 1 Pose: [0.767349820442101;-9.97369938841307;-6.65420795271277]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4846881450678;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76296352594841;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97409235611601;6.42335346880489]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.83704520511307;0.00253003695556726;0.00688343415761834;0.00252997841266994;1.83775660765077;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.085530e-284。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.072657e-283。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 674:
Mobile Platform 1 Pose: [0.767349820442101;-9.97248963588754;-6.66063498185093]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.67824854915314;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.95652393003375;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9737104897004;6.42250383859361]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.89814383161051;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.89885523414821;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.658364e-285。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.286067e-284。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 675:
Mobile Platform 1 Pose: [0.767349820442101;-9.97212638737025;-6.66230167170269]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.73636831191931;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.01464369279993;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97292593907962;6.4214515369986]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.02367193093576;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.02438333347346;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.494441e-285。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.936777e-284。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 676:
Mobile Platform 1 Pose: [0.767349820442101;-9.97377963025121;-6.653890637335]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47184945096649;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.7501248318471;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97452361317063;6.42390425329437]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76804407637406;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76875547891176;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.798536e-286。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.342878e-284。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 677:
Mobile Platform 1 Pose: [0.767349820442101;-9.97201076439136;-6.66310292739462]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.75486798854179;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.0331433694224;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97259177847144;6.42078290192004]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07713762824437;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07784903078207;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.796946e-286。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.720041e-285。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 678:
Mobile Platform 1 Pose: [0.767349820442101;-9.97263188810321;-6.65972432332045]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.65548819464618;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.93376357552679;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97418630581373;6.42344819349102]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.82201325347868;0.00253003695556726;0.00688343415761834;0.00252997841266994;1.82272465601638;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.097527e-287。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.338656e-285。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 679:
Mobile Platform 1 Pose: [0.767349820442101;-9.97364132545082;-6.65470815662913]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.49397821902798;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.7722535999086;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97358936226017;6.42225672771972]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91752422204826;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.91823562458596;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.542548e-287。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.166380e-286。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 680:
Mobile Platform 1 Pose: [0.767349820442101;-9.97170919919036;-6.66463964406125]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80311842070142;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08139380158203;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97303087101078;6.42157266756634]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.00688282194934;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.00759422448704;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.876803e-288。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.391803e-286。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 681:
Mobile Platform 1 Pose: [0.767349820442101;-9.97314663323438;-6.65711756072346]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.57312897365806;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.85140435453867;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9744907774131;6.42378367546192]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77329779757907;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.77400920011677;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.493322e-288。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.044344e-286。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 682:
Mobile Platform 1 Pose: [0.767349820442101;-9.97330053077479;-6.65655614674097]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.54850536719327;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.82678074807388;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97255449284006;6.420685863572]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08310332926533;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08381473180303;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.749131e-288。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.110945e-287。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 683:
Mobile Platform 1 Pose: [0.767349820442101;-9.97166281807087;-6.66486167070832]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81053939982001;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08881478070062;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97427090544103;6.42352684534043]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.80847731311029;0.00253003695556726;0.00688343415761834;0.00252997841266993;1.80918871564799;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.127105e-289。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.936525e-288。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 684:
Mobile Platform 1 Pose: [0.767349820442101;-9.97354625789404;-6.65512250504927]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50918902811246;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.78746440899307;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97346649848424;6.42200942461167]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93718242619625;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.93789382873395;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.865655e-289。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.423318e-288。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 685:
Mobile Platform 1 Pose: [0.767349820442101;-9.97281319366915;-6.6591260568138]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.6264793040959;-0.00246154026715742;-0.00729489637542035;-0.00246154026717589;-1.90475468497651;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97314323559109;6.42170471490821]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.98890448910024;0.00253003695556727;0.00688343415761829;0.00252997841266995;1.98961589163794;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.142768e-290。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.028309e-288。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 686:
Mobile Platform 1 Pose: [0.767349820442101;-9.97188288806309;-6.66372391871915]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77532820106453;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.05360358194514;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9744461814848;6.42364415747549]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.78043314610613;0.00253003695556725;0.00688343415761835;0.00252997841266993;1.78114454864383;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.273525e-290。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.335222e-289。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 687:
Mobile Platform 1 Pose: [0.767349820442101;-9.97375686298313;-6.65412199186792]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47549221385895;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75376759473956;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253289193397;6.42061354692008]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08655947423958;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08727087677728;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.151938e-290。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.692536e-289。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 688:
Mobile Platform 1 Pose: [0.767349820442101;-9.97228860995342;-6.66184886201299]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.71041269861198;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.98868807949259;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97434537090312;6.42358807599978]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.79656283917482;0.00253003695556726;0.00688343415761835;0.00252997841266993;1.79727424171251;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.503968e-291。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.145932e-290。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 689:
Mobile Platform 1 Pose: [0.767349820442101;-9.97231412058588;-6.66151733919034]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.70633099741844;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.98460637829906;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97334381599284;6.42176492100706]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.95681162481989;0.00253003695556727;0.0068834341576183;0.00252997841266994;1.95752302735759;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.507342e-291。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.955981e-290。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 690:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737512723993;-6.65425240266982]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47638670727164;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75466208815226;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97326112230981;6.42184468447878]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9700426141039;0.00253003695556727;0.00688343415761829;0.00252997841266994;1.9707540166416;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.392668e-292。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.809404e-291。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 691:
Mobile Platform 1 Pose: [0.767349820442101;-9.97186580304137;-6.66401877464914]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.77806180454004;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.05633718542065;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97439010473272;6.42348651182024]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.7894054264386;0.00253003695556726;0.00688343415761835;0.00252997841266993;1.7901168289763;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.469599e-292。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.005362e-291。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 692:
Mobile Platform 1 Pose: [0.767349820442101;-9.97284014101787;-6.65884156687581]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.62216772830075;-0.00246154026715742;-0.00729489637542035;-0.00246154026717589;-1.90044310918137;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97252736521428;6.42056634403159]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08744374938874;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08815515192644;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.646433e-293。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.405644e-291。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 693:
Mobile Platform 1 Pose: [0.767349820442101;-9.97353007212169;-6.65549486099173]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.51177875168885;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.79005413256947;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97440908279473;6.42363081363918]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.78636893651727;0.00253003695556726;0.00688343415761835;0.00252997841266993;1.78708033905497;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.141820e-293。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.345268e-292。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 694:
Mobile Platform 1 Pose: [0.767349820442101;-9.97165858941824;-6.66506747436925]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81121598424173;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08949136512234;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97322330365276;6.42152624942642]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.97609359923372;0.00253003695556727;0.00688343415761829;0.00252997841266994;1.97680500177142;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.100179e-293。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.651833e-292。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 695:
Mobile Platform 1 Pose: [0.767349820442101;-9.97332219961409;-6.65641025509312]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.54503835290511;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.82331373378572;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9733825470407;6.42198945911261]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.9506146571623;0.00253003695556726;0.0068834341576183;0.00252997841266994;1.9513260597;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.776097e-294。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.099449e-293。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 696:
Mobile Platform 1 Pose: [0.767349820442101;-9.97312231334473;-6.65767673310289]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.57702015600306;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.85529553688367;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97432293620406;6.42331174889799]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.80015239102423;0.00253003695556726;0.00688343415761835;0.00252997841266993;1.80086379356193;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.744300e-294。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.625213e-293。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 697:
Mobile Platform 1 Pose: [0.767349820442101;-9.97171883026691;-6.66474407966287]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80157744845446;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.07985282933508;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97253801254747;6.42054422571175]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08574017607977;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08645157861747;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.521586e-295。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.214170e-293。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 698:
Mobile Platform 1 Pose: [0.767349820442101;-9.97365358476948;-6.65477473175339]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.4920167280421;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.77029210892271;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97446156621346;6.42365424122229]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77797158952092;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.77868299205862;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.707478e-295。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.154031e-294。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 699:
Mobile Platform 1 Pose: [0.767349820442101;-9.97260470801336;-6.66038701201992]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.65983700902216;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.93811238990277;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97310696607334;6.42129640360793]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.99470761194009;0.00253003695556727;0.00688343415761829;0.00252997841266995;1.99541901447779;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.993347e-296。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.602400e-294。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 700:
Mobile Platform 1 Pose: [0.767349820442101;-9.97203190596772;-6.6631343028135]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.75148533632473;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.02976071720534;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97350550373426;6.42213587461796]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.93094158619246;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.93165298873015;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.856559e-296。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.906020e-295。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 701:
Mobile Platform 1 Pose: [0.767349820442101;-9.97378091484688;-6.65421099397882]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47164391565848;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.74991929653909;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97424519511967;6.42312109347325]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.81259096452676;0.00253003695556726;0.00688343415761835;0.00252997841266993;1.81330236706446;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.309853e-296。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.777705e-295。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 702:
Mobile Platform 1 Pose: [0.767349820442101;-9.97210354092507;-6.66297607933768]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.74002374314908;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.01829912402969;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97256464067816;6.42054673768992]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.08147967516875;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.08219107770645;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.059375e-297。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.089732e-295。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 703:
Mobile Platform 1 Pose: [0.767349820442101;-9.97251662044505;-6.66065988181471]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.67393101995236;-0.00246154026715742;-0.00729489637542033;-0.0024615402671759;-1.95220640083298;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97450246979546;6.42365777255846]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.77142701640074;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.77213841893844;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.641120e-297。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.349978e-296。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 704:
Mobile Platform 1 Pose: [0.767349820442101;-9.97368959323295;-6.65478611688946]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48625537388852;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.76453075476913;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97299676849292;6.42107826219293]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.01233922480814;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.01305062734584;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.030373e-297。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.013263e-296。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 705:
Mobile Platform 1 Pose: [0.767349820442101;-9.97175465483182;-6.66475884537983]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.79584551806792;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.07412089894853;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97362802032714;6.42228080295228]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91133893133193;0.00253003695556726;0.00688343415761831;0.00252997841266994;1.91205033386963;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.410161e-298。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.073374e-297。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 706:
Mobile Platform 1 Pose: [0.767349820442101;-9.97304120150316;-6.65799857994967]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.58999805065396;-0.00246154026715742;-0.00729489637542037;-0.00246154026717589;-1.86827343153458;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97415755175035;6.42291600006644]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.82661390361937;0.00253003695556726;0.00688343415761834;0.00252997841266994;1.82732530615707;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.137309e-298。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.241236e-297。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 707:
Mobile Platform 1 Pose: [0.767349820442101;-9.97338970220616;-6.65642815275008]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.53423793817461;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.81251331905522;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726067699604;6.42057301081789]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07473899001094;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07545039254864;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.544680e-299。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.545060e-298。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 708:
Mobile Platform 1 Pose: [0.767349820442101;-9.97164969681992;-6.66528134441591]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.81263879997345;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.09091418085407;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97453154569835;6.42364102832682]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76677487193848;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76748627447618;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.329687e-299。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.798121e-298。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 709:
Mobile Platform 1 Pose: [0.767349820442101;-9.97347399663502;-6.65582839786666]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.52075082955645;-0.00246154026715742;-0.00729489637542039;-0.00246154026717589;-1.79902621043706;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97289458743474;6.4208745232209]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.02868819411693;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.02939959665463;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 7.686424e-300。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.664425e-299。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 710:
Mobile Platform 1 Pose: [0.767349820442101;-9.97292668717239;-6.65888105439469]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60832034357805;-0.00246154026715742;-0.00729489637542036;-0.00246154026717589;-1.88659572445866;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9737482130554;6.42242123436172]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.8921080948104;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.8928194973481;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.884855e-300。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.503994e-299。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 711:
Mobile Platform 1 Pose: [0.767349820442101;-9.97181432276926;-6.66442565858921]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78629864807846;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06457402895907;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.974060846914;6.42269816602705]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.84208667743514;0.00253003695556726;0.00688343415761834;0.00252997841266994;1.84279807997284;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.829521e-301。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.927700e-300。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 712:
Mobile Platform 1 Pose: [0.767349820442101;-9.97372919651989;-6.65459360151873]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47991884797722;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75819422885783;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97266364970611;6.42062178289652]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.0656382306968;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.0663496332345;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.763346e-301。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.099098e-300。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 713:
Mobile Platform 1 Pose: [0.767349820442101;-9.97239865525981;-6.66162967657645]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.69280544959064;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.97108083047125;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454863212228;6.42360381433771]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76404104411026;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76475244664795;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.348732e-301。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.414754e-300。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 714:
Mobile Platform 1 Pose: [0.767349820442101;-9.97220781601706;-6.66240920816031]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.72333972843122;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.00161510931183;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97280217015318;6.42068765317281]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.04347495916601;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.04418636170371;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.714177e-302。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.540711e-301。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 715:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737712309178;-6.65447584641115]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47319334431187;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75146872519248;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97386433323751;6.42255434963483]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.87352886567302;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.87424026821072;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.599859e-302。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.126576e-301。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 716:
Mobile Platform 1 Pose: [0.767349820442101;-9.97194239293244;-6.66397669750168]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76580742197068;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04408280285129;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97395610803947;6.42246954054515]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.85884489736009;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.85955629989779;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.526323e-303。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.614616e-302。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 717:
Mobile Platform 1 Pose: [0.767349820442101;-9.97272517546807;-6.65977405227679]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.64056221626964;-0.00246154026715742;-0.00729489637542035;-0.0024615402671759;-1.91883759715025;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97273427879922;6.42069142724049]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.05433757579943;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.05504897833713;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.920648e-303。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.669845e-302。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 718:
Mobile Platform 1 Pose: [0.767349820442101;-9.97359559642795;-6.65548518512628]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50129486268809;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.77957024356871;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97455363980018;6.42354610424775]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.7632398156456;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.7639512181833;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.183388e-303。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.147169e-302。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 719:
Mobile Platform 1 Pose: [0.767349820442101;-9.97168165609405;-6.66530188218307]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80752531611292;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08580069699353;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97272110282929;6.42051984958603]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.05644573098839;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.05715713352609;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.565714e-304。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.196165e-303。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 720:
Mobile Platform 1 Pose: [0.767349820442101;-9.97322758485177;-6.65723396549874]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56017671487726;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.83845209575788;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97397480215622;6.42267757594965]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.85585383867978;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.85656524121748;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.625604e-304。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.201372e-303。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 721:
Mobile Platform 1 Pose: [0.767349820442101;-9.9732237462154;-6.65748858573452]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.56079089669615;-0.00246154026715742;-0.00729489637542038;-0.00246154026717589;-1.83906627757676;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97384455933323;6.42223232687744]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.87669269035784;0.00253003695556726;0.00688343415761833;0.00252997841266994;1.87740409289554;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 4.324579e-305。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 5.052746e-304。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 722:
Mobile Platform 1 Pose: [0.767349820442101;-9.97168281609899;-6.66528049011524]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.80733971532244;-0.00246154026715742;-0.00729489637542028;-0.0024615402671759;-2.08561509620305;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97281742815379;6.42077998305689]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.04103367906915;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.04174508160685;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.752861e-305。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.799613e-304。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 723:
Mobile Platform 1 Pose: [0.767349820442101;-9.97359796897122;-6.65539305800807]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.50091525576486;-0.00246154026715741;-0.0072948963754204;-0.00246154026717589;-1.77919063664548;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97454654361299;6.42346802852992]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.76437520559649;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.76508660813418;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.915856e-306。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 9.620390e-305。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 724:
Mobile Platform 1 Pose: [0.767349820442101;-9.97272065301078;-6.66013277969634]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.64128580943545;-0.00246154026715742;-0.00729489637542034;-0.0024615402671759;-1.91956119031606;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.9726527849889;6.42037301261035]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.0673765854518;0.00253003695556727;0.00688343415761827;0.00252997841266995;2.0680879879895;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.207054e-306。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.321283e-305。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 725:
Mobile Platform 1 Pose: [0.767349820442101;-9.97194560634691;-6.66392573791265]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.76529327565504;-0.00246154026715742;-0.0072948963754203;-0.0024615402671759;-2.04356865653565;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97407823215017;6.42278862350514]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.8393050396483;0.00253003695556726;0.00688343415761834;0.00252997841266994;1.840016442186;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.025435e-306。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.360557e-305。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 726:
Mobile Platform 1 Pose: [0.767349820442101;-9.97377182390404;-6.65458542841052]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.47309846651365;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75137384739426;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97372762321514;6.4219889740378]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.89540246925212;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.89611387178981;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.024701e-307。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.986670e-306。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 727:
Mobile Platform 1 Pose: [0.767349820442101;-9.97220379763053;-6.66280967705169]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.72398267027613;-0.00246154026715742;-0.00729489637542031;-0.0024615402671759;-2.00225805115674;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97291166318712;6.42088518516991]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.02595607373562;0.00253003695556727;0.00688343415761828;0.00252997841266995;2.02666747627332;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.660469e-307。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.251274e-306。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 728:
Mobile Platform 1 Pose: [0.767349820442101;-9.9724030828267;-6.66158740077672]
Robot Arm 1 Joint Angles: [-0.00246154026716315;1.69209703888798;-0.00246154026715742;-0.00729489637542032;-0.0024615402671759;-1.97037241976859;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.9745273800717;6.42336987067945]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.7674413722027;0.00253003695556725;0.00688343415761836;0.00252997841266993;1.7681527747404;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 3.880138e-308。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.388735e-307。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 729:
Mobile Platform 1 Pose: [0.767349820442101;-9.9737279647334;-6.6549146999261]
Robot Arm 1 Joint Angles: [-0.00246154026716313;1.48011593381668;-0.00246154026715741;-0.00729489637542041;-0.00246154026717589;-1.75839131469729;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97259840672069;6.4202487197529]
Robot Arm 2 Joint Angles: [0.00253002436772005;-2.07607710836511;0.00253003695556727;0.00688343415761826;0.00252997841266995;2.07678851090281;0.0025300234950514]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.117616e-308。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 1.738509e-307。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 730:
Mobile Platform 1 Pose: [0.767349820442101;-9.97181192261668;-6.66481706208585]
Robot Arm 1 Joint Angles: [-0.00246154026716316;1.78668267249125;-0.00246154026715742;-0.00729489637542029;-0.0024615402671759;-2.06495805337186;-0.0024615395541652]
Mobile Platform 2 Pose: [-2.34529194213667;9.97417343444809;6.42288550370665]
Robot Arm 2 Joint Angles: [0.00253002436772004;-1.82407267198077;0.00253003695556726;0.00688343415761834;0.00252997841266994;1.82478407451846;0.00253002349505138]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 6.041149e-309。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 8.743004e-308。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 731:
Mobile Platform 1 Pose: [0.767349820442101;-9.97293111309664;-6.65890443777963]
Robot Arm 1 Joint Angles: [-0.00246154026716314;1.60761219569787;-0.00246154026715742;-0.00729489637542036;-0.00246154026717589;-1.88588757657848;-0.00246153955416519]
Mobile Platform 2 Pose: [-2.34529194213667;9.97360691022397;6.42174215389036]
Robot Arm 2 Joint Angles: [0.00253002436772005;-1.91471654783891;0.00253003695556726;0.00688343415761832;0.00252997841266994;1.91542795037661;0.00253002349505139]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND = 2.419099e-309。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 
{无法识别的字段名称 "arm_4_6"。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_DCconstraint', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_DCconstraint.m', 40)" style="font-weight:bold">mobile_robot_DCconstraint</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_DCconstraint.m',40,0)">第 40 行</a>)
       obstacle_results.arm_4_6.closest.threat;...

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF>@(x)mobile_robot_DCconstraint(x,mobile_robot,obstacle_results,a_G,D_min,V_max,dt)', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 54)" style="font-weight:bold">mobile_robot_CBLF>@(x)mobile_robot_DCconstraint(x,mobile_robot,obstacle_results,a_G,D_min,V_max,dt)</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',54,0)">第 54 行</a>)
    fun2 = @(x) mobile_robot_DCconstraint(x,mobile_robot, obstacle_results, a_G, D_min, V_max, dt);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('fmincon', '/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/fmincon.m', 655)" style="font-weight:bold">fmincon</a> (<a href="matlab: opentoline('/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/fmincon.m',655,0)">第 655 行</a>)
        [ctmp,ceqtmp] = feval(confcn{3},X,varargin{:});

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 73)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',73,0)">第 73 行</a>)
    dx = fmincon(fun1, X, [], [], [], [], [], [], fun2, options);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
    [dx1] = mobile_robot_CBLF(mobile_robot1, target1, a1_G, obstacle_results1, D_min, V_max, dt);

原因:
    Failure in initial nonlinear constraint function evaluation. FMINCON cannot continue.
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.504961e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 Joint Angles: [-0.000121114227683022;0.185441233024398;-0.000121114227681381;1.86264489701995e-09;-0.000121114227682768;-0.22518584865809;-0.00012111422768375]
Mobile Platform 2 Pose: [-2.35629246996237;9.99866771779863;9.81844390803658]
Robot Arm 2 Joint Angles: [0.000121114227682458;-0.185441233024398;0.000121114227682772;-1.12819006464807e-09;0.000121114227682772;0.213165194053605;0.000121114227682772]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.993249e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 Joint Angles: [-0.000242257227143282;0.370882466048795;-0.00024225722714405;-0.000118135763669368;-0.000242257227146825;-0.450777934956321;-0.000242257227154676]
Mobile Platform 2 Pose: [-2.35629235164988;9.99733167866078;9.63690091192843]
Robot Arm 2 Joint Angles: [0.000242264350431372;-0.370882466048795;0.000242264350432035;8.24414026193607e-05;0.000242264350432035;0.426849011445395;0.000242264350432035]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.671567e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 Joint Angles: [-0.000363286279799942;0.556323699073193;-0.000363286279798916;-0.000357282809229051;-0.000363286279802673;-0.676844213293337;-0.000363286279809542]
Mobile Platform 2 Pose: [-2.35629223333216;9.99599115841255;9.4554542535664]
Robot Arm 2 Joint Angles: [0.00036341983930628;-0.556323699073193;0.000363419839307293;0.000250017340640077;0.000363419839307293;0.641164673097393;0.000363419839307293]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.504759e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 Joint Angles: [-0.000483866411480727;0.74176493209759;-0.000483866411478066;-0.000719510076232192;-0.000483866411482804;-0.903917115513842;-0.000483866411488692]
Mobile Platform 2 Pose: [-2.35629211520197;9.99464273480999;9.27450987248157]
Robot Arm 2 Joint Angles: [0.000484383292582506;-0.74176493209759;0.000484383292583868;0.000505155624687971;0.000484383292583868;0.856657309105139;0.000484383292583868]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.467838e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 Joint Angles: [-0.0006036745926073;0.927206165121988;-0.000603674592606945;-0.00120731922081321;-0.000603674592618552;-1.13251733087994;-0.000603674592617571]
Mobile Platform 2 Pose: [-2.35629199743552;9.99328288844238;9.09451173764292]
Robot Arm 2 Joint Angles: [0.000604974273389253;-0.927206165121988;0.000604974273390961;0.000851200159518876;0.000604974273390961;1.0738866812838;0.000604974273390961]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.543510e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346623]
Robot Arm 1 Joint Angles: [-0.000722796083224335;1.11264739814639;-0.00072279608322361;-0.00182553790636024;-0.000722796083242086;-1.36238137284088;-0.000722796083234236]
Mobile Platform 2 Pose: [-2.35629187984574;9.99191235934184;8.91540928762626]
Robot Arm 2 Joint Angles: [0.000725384345597788;-1.11264739814639;0.000725384345599841;0.00129364154192713;0.000725384345599841;1.29272889389694;0.000725384345599841]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528709;-9.99002831581144;-8.79949883355178]
Robot Arm 1 Joint Angles: [-0.000841580556365119;1.29808863117078;-0.000841580556358477;-0.00257974514714843;-0.000841580556377935;-1.59288971083287;-0.000841580556369103]
Mobile Platform 2 Pose: [-2.35629176199879;9.9905342833261;8.73682081499212]
Robot Arm 2 Joint Angles: [0.000846057762596529;-1.29808863117078;0.000846057762598927;0.00183860222856144;0.000846057762598927;1.51267609364503;0.000846057762598927]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274173]
Robot Arm 1 Joint Angles: [-0.000959829987060112;1.48352986419518;-0.000959829987047145;-0.00347025803777765;-0.000959829987065621;-1.82450740028498;-0.00095982998705679]
Mobile Platform 2 Pose: [-2.35611025158557;9.9891465482355;8.5590054865483]
Robot Arm 2 Joint Angles: [0.000967010451310628;-1.48352986419518;0.000967010451313383;0.00248884090661895;0.000967010451313383;1.73406357390168;0.000967010451313383]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196129;-9.98711511781;-8.46062525935787]
Robot Arm 1 Joint Angles: [-0.00107743739451576;1.66897109721958;-0.0010774373945052;-0.00449854422257743;-0.00107743739452368;-2.05708258800973;-0.00107743739451485]
Mobile Platform 2 Pose: [-2.35593389444046;9.98774923775423;8.38210572884157]
Robot Arm 2 Joint Angles: [0.00108832850473398;-1.66897109721958;0.00108832850473708;0.00324800741830019;0.00108832850473708;1.95687408391696;0.00108832850473708]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929674;-9.98565931998688;-8.29072857539468]
Robot Arm 1 Joint Angles: [-0.00119546838174874;1.85441233024398;-0.00119546838174059;-0.00567665166884611;-0.00119546838175906;-2.0943951023932;-0.00119546838175023]
Mobile Platform 2 Pose: [-2.35574402673921;9.98635048066111;8.20503419253046]
Robot Arm 2 Joint Angles: [0.00121095860422302;-1.85441233024398;0.00121095860422218;0.00412545279001913;0.00121095860422652;2.0943951023932;0.00121095860422652]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.9841823282427;-8.12462828169497]
Robot Arm 1 Joint Angles: [-0.00131696123650541;2.03985356326837;-0.00131696123649832;-0.00639922083539073;-0.00131696123651679;-2.30399143679527;-0.00131696052350609]
Mobile Platform 2 Pose: [-2.35560556141365;9.98493698756494;8.0301676625192]
Robot Arm 2 Joint Angles: [0.00133404637364525;-2.03985356326837;0.00133404637364385;0.00485301900520203;0.00133404637364907;2.29681394327048;0.00133404606251552]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840344088;-7.9599482888563]
Robot Arm 1 Joint Angles: [-0.00143870942785493;2.14949927567626;-0.00143870942784766;-0.00719336673714352;-0.00143870942786614;-2.22826414941097;-0.00143870871485544]
Mobile Platform 2 Pose: [-2.35468201877975;9.98351837389537;7.85622998501692]
Robot Arm 2 Joint Angles: [0.00145817172991377;-2.146360029523;0.00145817172990789;0.00563275204773398;0.00145817172991792;2.2229803029158;0.00145817141878437]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455234098;-7.79314279536571]
Robot Arm 1 Joint Angles: [-0.00156630119507468;2.18077941225243;-0.00156630119506725;-0.00742071448357543;-0.00156630119508573;-2.20427246494267;-0.00156630048207503]
Mobile Platform 2 Pose: [-2.35419810742336;9.98211893325932;7.68192331325391]
Robot Arm 2 Joint Angles: [0.00158487588019002;-2.1772249970916;0.00158487588017968;0.00585697080358425;0.00158487588019451;2.20007654707362;0.00158487556906097]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.9797235609508;-7.63794018922978]
Robot Arm 1 Joint Angles: [-0.00168692949029622;2.19333238380943;-0.00168692949028909;-0.00746871767647478;-0.00168692949030756;-2.20035149691845;-0.00168692877729686]
Mobile Platform 2 Pose: [-2.35381265691251;9.98066881370734;7.51575626629677]
Robot Arm 2 Joint Angles: [0.0017066572920208;-2.18886529366032;0.00170665729200596;0.00590853627008456;0.00170665729202561;2.19568981859271;0.00170665698089206]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591665116;-7.46636918684545]
Robot Arm 1 Joint Angles: [-0.00182021652145274;2.1916426546638;-0.00182021652144546;-0.00746685601467987;-0.00182021652146393;-2.1937495063759;-0.00182021580845323]
Mobile Platform 2 Pose: [-2.35325692862503;9.97930446571077;7.33822200512307]
Robot Arm 2 Joint Angles: [0.00183803257631036;-2.18822572479515;0.00183803257629106;0.00591142524002384;0.00183803257631551;2.19027134811059;0.00183803226518197]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346212;-9.97678818484225;-7.31817266015123]
Robot Arm 1 Joint Angles: [-0.00193743410109527;2.19689567720951;-0.00193743410108827;-0.0074507970691896;-0.00193743410110675;-2.19753736887274;-0.00193743338809605]
Mobile Platform 2 Pose: [-2.35240573029189;9.97782824245242;7.17880315012759]
Robot Arm 2 Joint Angles: [0.00195660291004402;-2.19339952858518;0.00195660291002022;0.00589982486088515;0.00195660291004948;2.19402017658961;0.00195660259891594]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232725;-9.9753590119579;-7.16418511934567]
Robot Arm 1 Joint Angles: [-0.00205940500907929;2.19373921813631;-0.00205940500907258;-0.00742961334509667;-0.00205940500909106;-2.19394443665204;-0.00205940429608036]
Mobile Platform 2 Pose: [-2.35116182734722;9.97645060174499;7.01005855977249]
Robot Arm 2 Joint Angles: [0.00208329149598775;-2.19021811373217;0.00208329149595947;0.00588290491452782;0.00208329149599353;2.19041446088363;0.00208329118485999]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.78539602676002;-9.97430990779222;-7.00084677780109]
Robot Arm 1 Joint Angles: [-0.00218788775603952;2.1745482189954;-0.00218788775603313;-0.00740588686305206;-0.00218788775605161;-2.17462402122166;-0.00218788704304091]
Mobile Platform 2 Pose: [-2.35099353113615;9.97523754450294;6.84195865363499]
Robot Arm 2 Joint Angles: [0.00221054700870913;-2.26975374786376;0.00221054700867861;0.00586455303854629;0.00221054715542499;2.26997309388299;0.00221054669758177]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.78539595660333;-9.97259808478388;-6.91581468798367]
Robot Arm 1 Joint Angles: [-0.00225972690260118;2.20060448127733;-0.00225972690259496;-0.00739236789516434;-0.00225972690261343;-2.20063535217658;-0.00225972618960273]
Mobile Platform 2 Pose: [-2.35083713276682;9.97363422426754;6.72931239863888]
Robot Arm 2 Joint Angles: [0.00229598685117849;-2.42046586162583;0.00229598208659332;0.00585237833645952;0.00229596743454776;2.43619647855806;0.00229598177549648]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227853;-9.97409553094106;-6.77330520679708]
Robot Arm 1 Joint Angles: [-0.00236353352892908;2.01516324825293;-0.00236353352892313;-0.00737275824162398;-0.0023635335289416;-1.96106357862124;-0.0023635328159309]
Mobile Platform 2 Pose: [-2.35019751194058;9.97448136838889;6.56763386443262]
Robot Arm 2 Joint Angles: [0.00242431476276365;-2.23502462860144;0.00242430999817819;0.0058838019797557;0.00242429534613263;2.14167216728402;0.00242431020615072]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.97228129699828;-6.73833367577277]
Robot Arm 1 Joint Angles: [-0.0023973975264219;2.09439509667864;-0.00239739752641602;-0.00731935067018818;-0.00239739752643449;-2.09439509417083;-0.00239739681342379]
Mobile Platform 2 Pose: [-2.34783742717047;9.97259570522482;6.5135822892226]
Robot Arm 2 Joint Angles: [0.00246427912773896;-2.22706753043393;0.0024642743631536;0.00578374426384798;0.00246425971110804;2.19912180431797;0.00246427457112614]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540074;-9.97333331315079;-6.69737167009219]
Robot Arm 1 Joint Angles: [-0.00242465195277759;1.92606732656943;-0.00242465195277178;-0.00731416496329334;-0.00242465195279026;-1.9260776954754;-0.00242465123977956]
Mobile Platform 2 Pose: [-2.34780267049148;9.97428681920161;6.47077670770765]
Robot Arm 2 Joint Angles: [0.00250008949267869;-2.04162629740954;0.00250008472809341;0.00575316607069934;0.00250007007604786;1.92857408529499;0.00250008493606595]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395781195851;-9.97294909644746;-6.68023583759776]
Robot Arm 1 Joint Angles: [-0.00243934043716969;1.98753916361096;-0.00243934043716392;-0.00731132947224667;-0.0024393404371824;-1.98755520349902;-0.0024393397241717]
Mobile Platform 2 Pose: [-2.34780265403097;9.97308986330296;6.44692143458113]
Robot Arm 2 Joint Angles: [0.00251694319663578;-2.0943951023932;0.00251693843205055;0.00570177375790443;0.00251692378000499;2.0943951023932;0.00251693864002308]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395772638942;-9.97200303864331;-6.67347995257358]
Robot Arm 1 Joint Angles: [-0.00244810084900341;2.0943951023932;-0.00244810084899765;-0.00730965080542708;-0.00244810084901613;-2.0943951023932;-0.00244810013600543]
Mobile Platform 2 Pose: [-2.34780264363309;9.97332802351795;6.43383294868015]
Robot Arm 2 Joint Angles: [0.0025275906211904;-2.05628790097928;0.00252758585660519;0.00570020674268472;0.00252757120455963;2.05629103500972;0.00252758606457773]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395767401621;-9.97358488642586;-6.65848980933812]
Robot Arm 1 Joint Angles: [-0.00245346200287458;1.90895386925231;-0.00245346200286885;-0.00730863212378707;-0.00245346200288733;-1.84130047961619;-0.00245346128987663]
Mobile Platform 2 Pose: [-2.34780263757901;9.97392952050267;6.42664907547555]
Robot Arm 2 Joint Angles: [0.00253378999783812;-1.96004746849453;0.00253378523325293;0.00569929181306756;0.00253377058120737;1.96005243238421;0.00253378544122546]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395764311851;-9.9716500757212;-6.66429501601706]
Robot Arm 1 Joint Angles: [-0.00245662406489679;2.0943950978115;-0.00245662406489105;-0.00730248004082667;-0.00245662406490952;-2.09439510583112;-0.00245662335189882]
Mobile Platform 2 Pose: [-2.3478026340673;9.97235433373431;6.41958555185207]
Robot Arm 2 Joint Angles: [0.00253738412430894;-2.09439510781682;0.00253737935972376;0.00569875846231868;0.00253736470767821;2.09439509504785;0.0025373795676963]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785395762440954;-9.97334587567019;-6.65299730614471]
Robot Arm 1 Joint Angles: [-0.00245853800054772;1.90895386479396;-0.002458538000542;-0.00730211760364607;-0.00245853800056048;-1.82306748031366;-0.00245853728754978]
Mobile Platform 2 Pose: [-2.34512155332381;9.97427845738985;6.41987846291419]
Robot Arm 2 Joint Angles: [0.00253954821370307;-1.90895387518525;0.00253954344911789;0.0056984415750499;0.00253952879707233;1.79987078379954;0.00253954365709043]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785395761295571;-9.9725430019079;-6.65580126947701]
Robot Arm 1 Joint Angles: [-0.00245971087207978;2.03741082995867;-0.00245971087207406;-0.0072992808023168;-0.00245971087209253;-1.95153011908103;-0.00245971015908183]
Mobile Platform 2 Pose: [-2.3421080956285;9.97262143390046;6.41535464261109]
Robot Arm 2 Joint Angles: [0.00254087594210204;-2.09439510358243;0.00254087117751687;0.00569447220800087;0.00254085652547131;2.06499851135744;0.00254087138548941]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395760593853;-9.97237811570277;-6.65550571971024]
Robot Arm 1 Joint Angles: [-0.0024604294316762;2.06379090445736;-0.00246042943167048;-0.00729756247915696;-0.00246042943168896;-1.97791363022604;-0.00246042871867826]
Mobile Platform 2 Pose: [-2.34210809485161;9.97370774879315;6.41623415907743]
Robot Arm 2 Joint Angles: [0.00254167147825551;-1.92058400422124;0.00254166671367034;0.00569375567697825;0.00254165206162478;1.8911888450583;0.00254166692164288]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395760172427;-9.97341831965915;-6.64975907402211]
Robot Arm 1 Joint Angles: [-0.00246086097205636;1.89735724181491;-0.00246086097205065;-0.00729653285710986;-0.00246086097206912;-1.81148202682769;-0.00246086025905842]
Mobile Platform 2 Pose: [-2.34210809438132;9.97368504390152;6.41534127871149]
Robot Arm 2 Joint Angles: [0.00254215305269678;-1.92421634636591;0.00254214828811161;0.00569331516123828;0.00254213363606605;1.89482206823445;0.00254214849608414]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395759909198;-9.97165700587365;-6.65837171221346]
Robot Arm 1 Joint Angles: [-0.00246113051844891;2.17916679493719;-0.00246113051844318;-0.00729588029970588;-0.00246113051846165;-2.09329288506478;-0.00246112980545095]
Mobile Platform 2 Pose: [-2.34210809409183;9.97262138442325;6.41343982601175]
Robot Arm 2 Joint Angles: [0.00254244949189608;-2.09440159182185;0.00254244472731091;0.00569304409482356;0.00254243007526535;2.06500785582321;0.00254244493528345]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395759753156;-9.97359339940187;-6.64819520364775]
Robot Arm 1 Joint Angles: [-0.00246129030556191;1.869343452737;-0.0024612903055562;-0.00729550261551014;-0.00246129030557468;-1.78347029823298;-0.00246128959256398]
Mobile Platform 2 Pose: [-2.34210809392036;9.97433246733537;6.41587076289657]
Robot Arm 2 Joint Angles: [0.00254262508096128;-1.82062816774668;0.00254262031637611;0.00569288595806916;0.00254260566433055;1.79123474802155;0.00254262052434864]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785395759655636;-9.97198118931604;-6.65650177558254]
Robot Arm 1 Joint Angles: [-0.00246139016627963;2.12729682409647;-0.0024613901662739;-0.0072952602415042;-0.00246139016629238;-2.04142415434046;-0.00246138945328168]
Mobile Platform 2 Pose: [-2.3421080938134;9.97241697171521;6.41261510590173]
Robot Arm 2 Joint Angles: [0.00254273460481264;-2.1271073658327;0.00254272984022748;0.00569278481901898;0.00254271518818192;2.09771414838568;0.00254273004820002]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395759597111;-9.97295374746973;-6.65125334903681]
Robot Arm 1 Joint Angles: [-0.00246145009563331;1.97168737727154;-0.00246145009562759;-0.00729511800759162;-0.00246145009564607;-1.88581499198336;-0.00246144938263537]
Mobile Platform 2 Pose: [-2.34210809374991;9.97397239199167;6.41513645983941]
Robot Arm 2 Joint Angles: [0.00254279961653077;-1.87824006321155;0.0025427948519456;0.00569272643084828;0.00254278019990005;1.84884696254087;0.00254279505991814]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785395759561427;-9.97296922404441;-6.6513492560343]
Robot Arm 1 Joint Angles: [-0.00246148663562581;1.96921103745212;-0.0024614866356201;-0.00729503013686692;-0.00246148663563858;-1.88333882790539;-0.00246148592262787]
Mobile Platform 2 Pose: [-2.34210809371104;9.97336115283498;6.41387617138006]
Robot Arm 2 Joint Angles: [0.00254283942761043;-1.97603829172241;0.00254283466302527;0.00569268987067677;0.00254282001097971;1.94664526417207;0.0025428348709978]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785395759539366;-9.97196650325642;-6.6562907865685]
Robot Arm 1 Joint Angles: [-0.00246150922616748;2.1296463091937;-0.00246150922616176;-0.00729497579976558;-0.00246150922618023;-2.04377420832117;-0.00246150851316953]
Mobile Platform 2 Pose: [-2.34210809368731;9.97289706207257;6.4133101147757]
Robot Arm 2 Joint Angles: [0.00254286372518275;-2.05029279159588;0.00254285896059758;0.00569266775931735;0.00254284430855202;2.02089980826825;0.00254285916857012]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.78539575952626;-9.97359580921727;-6.64798611087732]
Robot Arm 1 Joint Angles: [-0.00246152264733956;1.86895732361144;-0.00246152264733385;-0.0072949439527453;-0.00246152264735233;-1.78308528643295;-0.00246152193434162]
Mobile Platform 2 Pose: [-2.34210809367321;9.97424171812654;6.41530523422694]
Robot Arm 2 Joint Angles: [0.0025428781635639;-1.83514780990916;0.00254287339897873;0.00569265470678555;0.00254285874693317;1.8057548526866;0.00254287360695126]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785395759518025;-9.97165958807069;-6.65794808034011]
Robot Arm 1 Joint Angles: [-0.00246153108011986;2.178752686596;-0.00246153108011414;-0.00729492348487617;-0.00246153108013261;-2.09288069035325;-0.00246153036712191]
Mobile Platform 2 Pose: [-2.34210809366441;9.97233214672094;6.41227241904428]
Robot Arm 2 Joint Angles: [0.00254288717764754;-2.14067922649241;0.00254288241306238;0.00569264639457879;0.00254286776101682;2.11128628589426;0.00254288262103492]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785395759513141;-9.97340627578782;-6.64887127655015]
Robot Arm 1 Joint Angles: [-0.00246153608088411;1.89928264003497;-0.0024615360808784;-0.00729491166461823;-0.00246153608089688;-1.81341066743274;-0.00246153536788618]
Mobile Platform 2 Pose: [-2.34210809365921;9.97417816346664;6.41528204795851]
Robot Arm 2 Joint Angles: [0.00254289250119615;-1.84531654240499;0.00254288773661098;0.00569264161882115;0.00254287308456542;1.81592361135836;0.00254288794458352]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785395759510112;-9.97238834370791;-6.65431341078414]
Robot Arm 1 Joint Angles: [-0.00246153918221748;2.06215176531351;-0.00246153918221176;-0.00729490415802862;-0.00246153918223023;-1.97627980772445;-0.00246153846921953]
Mobile Platform 2 Pose: [-2.342108093656;9.97303022760168;6.41323609411338]
Robot Arm 2 Joint Angles: [0.0025428957897949;-2.02898627776894;0.00254289102520973;0.00569263858925905;0.00254287637316417;1.99959335278143;0.00254289123318227]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785395759508273;-9.97251220481971;-6.6534707095392]
Robot Arm 1 Joint Angles: [-0.00246154106582999;2.04233398293087;-0.00246154106582428;-0.00729489966353731;-0.00246154106584275;-1.9564620343308;-0.00246154035283205]
Mobile Platform 2 Pose: [-2.34210809365405;9.97321999860564;6.4137412136075]
Robot Arm 2 Joint Angles: [0.00254289777754848;-1.99862291533485;0.00254289301296331;0.00569263678893252;0.00254287836091775;1.96922999394799;0.00254289322093585]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785395759507166;-9.97332888203844;-6.64945225394588]
Robot Arm 1 Joint Angles: [-0.00246154219933972;1.91166562522433;-0.00246154219933401;-0.00729489695433561;-0.00246154219935249;-1.82579368204266;-0.00246154148634179]
Mobile Platform 2 Pose: [-2.34210809365289;9.97406808633442;6.41491052053024]
Robot Arm 2 Joint Angles: [0.0025428989674008;-1.86292887764999;0.00254289420281563;0.0056926357083512;0.00254287955077007;1.8335359584243;0.00254289441078817]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785395759506474;-9.97169930686996;-6.65771324223667]
Robot Arm 1 Joint Angles: [-0.00246154290748153;2.1723976504694;-0.00246154290747581;-0.00729489524224964;-0.00246154290749428;-2.08652571071191;-0.00246154219448358]
Mobile Platform 2 Pose: [-2.34210809365217;9.97236642212424;6.41229604271917]
Robot Arm 2 Joint Angles: [0.00254289970766841;-2.13519515059764;0.00254289494308324;0.00569263502754362;0.00254288029103769;2.10580223273357;0.00254289515105578]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785395759506065;-9.97361778204734;-6.64785910460435]
Robot Arm 1 Joint Angles: [-0.00246154332633321;1.86544162109776;-0.0024615433263275;-0.00729489425172447;-0.00246154332634598;-1.77956968332132;-0.00246154261333528]
Mobile Platform 2 Pose: [-2.34210809365174;9.9743040070447;6.41540117536834]
Robot Arm 2 Joint Angles: [0.00254290014414449;-1.82518156293177;0.00254289537955932;0.00569263463580369;0.00254288072751376;1.79578864585117;0.00254289558753186]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785395759505809;-9.97187540409097;-6.656938911363]
Robot Arm 1 Joint Angles: [-0.00246154358894332;2.14422209348063;-0.00246154358893759;-0.00729489361411645;-0.00246154358895607;-2.05835015697939;-0.00246154287594537]
Mobile Platform 2 Pose: [-2.34210809365148;9.97272882892248;6.41272608689594]
Robot Arm 2 Joint Angles: [0.00254290041553091;-2.07721006223758;0.00254289565094574;0.00569263438530757;0.00254288099890018;2.04781714565798;0.00254289585891828]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785395759505656;-9.97307917302321;-6.65059553927168]
Robot Arm 1 Joint Angles: [-0.00246154374588661;1.95161906394996;-0.00246154374588089;-0.00729489324214186;-0.00246154374589937;-1.86574712819268;-0.00246154303288867]
Mobile Platform 2 Pose: [-2.34210809365132;9.9735494001758;6.41421770604469]
Robot Arm 2 Joint Angles: [0.00254290057810075;-1.94591866155839;0.00254289581351558;0.00569263423868453;0.00254288116147002;1.91652574527203;0.00254289602148812]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785395759505562;-9.97283566172529;-6.65207568829744]
Robot Arm 1 Joint Angles: [-0.00246154384200664;1.99058087138579;-0.00246154384200093;-0.00729489301058375;-0.0024615438420194;-1.90470893609162;-0.0024615431290087]
Mobile Platform 2 Pose: [-2.34210809365122;9.97382016867922;6.41443080527234]
Robot Arm 2 Joint Angles: [0.00254290067629718;-1.90259570092293;0.00254289591171201;0.0056926341490769;0.00254288125966646;1.87320278481579;0.00254289611968455]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036872e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782991e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785395759505504;-9.97208304294275;-6.65574885789258]
Robot Arm 1 Joint Angles: [-0.00246154390119389;2.11099987644941;-0.00246154390118817;-0.00729489286848789;-0.00246154390120664;-2.02512794143944;-0.00246154318819594]
Mobile Platform 2 Pose: [-2.34210809365116;9.97251452749961;6.41250878136497]
Robot Arm 2 Joint Angles: [0.00254290073700335;-2.11149828960361;0.00254289597241818;0.00569263409345631;0.00254288132037262;2.08210537360771;0.00254289618039072]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.811171e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.141104e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.78539575950547;-9.97355589919412;-6.64829011432792]
Robot Arm 1 Joint Angles: [-0.00246154393643776;1.87534287614746;-0.00246154393643205;-0.00729489278473175;-0.00246154393645053;-1.78947094130499;-0.00246154322343983]
Mobile Platform 2 Pose: [-2.34210809365113;9.97434345693871;6.41538148692137]
Robot Arm 2 Joint Angles: [0.0025429007728373;-1.81886957931598;0.00254289600825213;0.00569263406123155;0.00254288135620657;1.78947666338453;0.00254289621622467]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563931e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.132099e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785395759505448;-9.97163424244322;-6.65815397495836]
Robot Arm 1 Joint Angles: [-0.0024615439585739;2.18280795623652;-0.00246154395856817;-0.00729489273103497;-0.00246154395858665;-2.09693602150146;-0.00246154324557594]
Mobile Platform 2 Pose: [-2.34210809365111;9.97249455243078;6.41233877520899]
Robot Arm 2 Joint Angles: [0.00254290079520431;-2.11469430056367;0.00254289603061915;0.00569263404057355;0.00254288137857359;2.08530138467353;0.00254289623859168]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056079e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.165417e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785395759505435;-9.97348102378628;-6.64859180451582]
Robot Arm 1 Joint Angles: [-0.00246154397168094;1.88732294131718;-0.00246154397167523;-0.0072948927000638;-0.00246154397169371;-1.80145100664405;-0.00246154325868301]
Mobile Platform 2 Pose: [-2.34210809365109;9.97384857164013;6.4146437806087]
Robot Arm 2 Joint Angles: [0.00254290080850408;-1.8980512270569;0.00254289604391891;0.00569263402861657;0.00254288139187335;1.86865831119068;0.00254289625189145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231848e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.659173e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785395759505427;-9.97225080614539;-6.65511445526811]
Robot Arm 1 Joint Angles: [-0.00246154397983688;2.08415776383889;-0.00246154397983116;-0.00729489268030123;-0.00246154397984964;-1.99828582920529;-0.00246154326683894]
Mobile Platform 2 Pose: [-2.34210809365109;9.97351643060304;6.41387943220112]
Robot Arm 2 Joint Angles: [0.00254290081661526;-1.95119379298377;0.00254289605203009;0.00569263402118171;0.00254288139998454;1.92180087713242;0.00254289626000263]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531990e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402612e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785395759505423;-9.97265434030515;-6.65284295629283]
Robot Arm 1 Joint Angles: [-0.00246154398476928;2.01959229826642;-0.00246154398476357;-0.00729489266855363;-0.00246154398478204;-1.93372036365632;-0.00246154327177134]
Mobile Platform 2 Pose: [-2.34210809365108;9.97275725686395;6.41286580311677]
Robot Arm 2 Joint Angles: [0.00254290082158741;-2.07266159123301;0.00254289605700225;0.00569263401664662;0.00254288140495669;2.04326867539072;0.00254289626497478]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692260e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.092890e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.78539575950542;-9.97322813797382;-6.65009145689996]
Robot Arm 1 Joint Angles: [-0.00246154398774938;1.92778467127224;-0.00246154398774367;-0.00729489266141704;-0.00246154398776215;-1.84191273667641;-0.00246154327475145]
Mobile Platform 2 Pose: [-2.34210809365108;9.97429502481787;6.41521457143538]
Robot Arm 2 Joint Angles: [0.00254290082453367;-1.82661871860346;0.0025428960599485;0.00569263401398839;0.00254288140790294;1.79722580276649;0.00254289626792104]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663859e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.038692e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.785395759505418;-9.97176377613675;-6.65747813759201]
Robot Arm 1 Joint Angles: [-0.00246154398960598;2.16208256519771;-0.00246154398960025;-0.00729489265693481;-0.00246154398961873;-2.07621063061084;-0.00246154327660803]
Mobile Platform 2 Pose: [-2.34210809365108;9.97235747256093;6.4121103428842]
Robot Arm 2 Joint Angles: [0.00254290082637444;-2.1366270797121;0.00254289606178927;0.00569263401228948;0.00254288140974371;2.10723416387853;0.00254289626976181]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984807e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.087005e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785395759505417;-9.97362810088841;-6.64792805207277]
Robot Arm 1 Joint Angles: [-0.00246154399070493;1.86379060493077;-0.00246154399069922;-0.0072948926543339;-0.0024615439907177;-1.7779186703491;-0.002461543277707]
Mobile Platform 2 Pose: [-2.34210809365108;9.97408953513251;6.414970293288]
Robot Arm 2 Joint Angles: [0.00254290082746323;-1.85949706825727;0.00254289606287806;0.00569263401131247;0.0025428814108325;1.83010415242566;0.0025428962708506]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059253e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.365814e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785395759505416;-9.97178688319742;-6.65748565585436]
Robot Arm 1 Joint Angles: [-0.0024615439913949;2.15838543548645;-0.00246154399138918;-0.00729489265265837;-0.00246154399140765;-2.07251350090813;-0.00246154327839695]
Mobile Platform 2 Pose: [-2.34210809365107;9.97318588117984;6.41330416576084]
Robot Arm 2 Joint Angles: [0.00254290082813332;-2.00408170068363;0.00254289606354815;0.00569263401069596;0.00254288141150259;1.97468878485325;0.00254289627152069]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857436e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.453842e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785395759505416;-9.97319545975326;-6.65010797578431]
Robot Arm 1 Joint Angles: [-0.00246154399180599;1.93301318655206;-0.00246154399180028;-0.00729489265168518;-0.00246154399181875;-1.84714125197569;-0.00246154327880805]
Mobile Platform 2 Pose: [-2.34210809365107;9.97306345077843;6.41331716254792]
Robot Arm 2 Joint Angles: [0.00254290082854018;-2.02367056490889;0.00254289606395501;0.00569263401032664;0.00254288141190946;1.99427764907925;0.00254289627192755]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905819e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.897975e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785395759505415;-9.97269674450398;-6.65290069318509]
Robot Arm 1 Joint Angles: [-0.00246154399205884;2.01280762643513;-0.00246154399205312;-0.00729489265107502;-0.0024615439920716;-1.92693569185998;-0.0024615432790609]
Mobile Platform 2 Pose: [-2.34210809365107;9.97416048672452;6.41490789560543]
Robot Arm 2 Joint Angles: [0.0025429008287828;-1.84814481353448;0.00254289606419763;0.00569263401010684;0.00254288141215207;1.81875189770527;0.00254289627217017]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634522e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.328578e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785395759505415;-9.97221078172521;-6.65519569974022]
Robot Arm 1 Joint Angles: [-0.00246154399221388;2.09056167103909;-0.00246154399220816;-0.00729489265070351;-0.00246154399222663;-2.00468973646468;-0.00246154327921593]
Mobile Platform 2 Pose: [-2.34210809365107;9.97233531843931;6.41206279596258]
Robot Arm 2 Joint Angles: [0.00254290082893408;-2.14017173916872;0.00254289606434891;0.00569263400996744;0.00254288141230335;2.1107788233398;0.00254289627232145]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.067005e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.828611e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735006134901;-6.64870124197991]
Robot Arm 1 Joint Angles: [-0.00246154399230644;1.8841885886564;-0.00246154399230073;-0.0072948926504832;-0.00246154399231921;-1.79831665408243;-0.00246154327930851]
Mobile Platform 2 Pose: [-2.34210809365107;9.97425482411529;6.41516709728715]
Robot Arm 2 Joint Angles: [0.00254290082902329;-1.8330508310118;0.00254289606443812;0.00569263400988749;0.00254288141239256;1.80365791518304;0.00254289627241065]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.069100e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.488361e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163094321734;-6.65826420090662]
Robot Arm 1 Joint Angles: [-0.00246154399236456;2.18333583229786;-0.00246154399235883;-0.00729489265034229;-0.00246154399237731;-2.09746389772418;-0.0024615432793666]
Mobile Platform 2 Pose: [-2.34210809365107;9.97286569299647;6.4127631988984]
Robot Arm 2 Joint Angles: [0.00254290082907862;-2.05531181002324;0.00254289606449345;0.00569263400983639;0.0025428814124479;2.02591889419458;0.00254289627246599]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.776572e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.703991e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785395759505415;-9.97354092061521;-6.64840078543374]
Robot Arm 1 Joint Angles: [-0.00246154399239889;1.87773944863744;-0.00246154399239318;-0.00729489265026122;-0.00246154399241166;-1.79186751406391;-0.00246154327940096]
Mobile Platform 2 Pose: [-2.34210809365107;9.97339476804833;6.41380283512242]
Robot Arm 2 Joint Angles: [0.0025429008291119;-1.97065980172466;0.00254289606452673;0.00569263400980635;0.00254288141248117;1.94126688589606;0.00254289627249926]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247228e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.540940e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.785395759505415;-9.97212038216775;-6.6558824117393]
Robot Arm 1 Joint Angles: [-0.00246154399242036;2.10502560023202;-0.00246154399241463;-0.00729489265020912;-0.00246154399243311;-2.0191536656586;-0.00246154327942241]
Mobile Platform 2 Pose: [-2.34210809365107;9.97394616652996;6.41447892096625]
Robot Arm 2 Joint Angles: [0.0025429008291319;-1.88243604466486;0.00254289606454673;0.00569263400978816;0.00254288141250118;1.85304312883629;0.00254289627251927]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064539e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.495301e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785395759505415;-9.97279446002223;-6.65223019561338]
Robot Arm 1 Joint Angles: [-0.00246154399243326;1.99717314351447;-0.00246154399242755;-0.00729489265017845;-0.00246154399244602;-1.91130120894111;-0.00246154327943532]
Mobile Platform 2 Pose: [-2.34210809365107;9.97243096338101;6.41219684503508]
Robot Arm 2 Joint Angles: [0.00254290082914432;-2.12486854849564;0.00254289606455916;0.00569263400977669;0.0025428814125136;2.0954756326671;0.0025428962725317]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.742991e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.506671e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.785395759505415;-9.97311524782296;-6.65079308472694]
Robot Arm 1 Joint Angles: [-0.00246154399244109;1.94584709539847;-0.00246154399243538;-0.00729489265015967;-0.00246154399245386;-1.85997516082515;-0.00246154327944316]
Mobile Platform 2 Pose: [-2.34210809365107;9.9743356932515;6.41521899496435]
Robot Arm 2 Joint Angles: [0.00254290082915163;-1.82011176921767;0.00254289606456646;0.00569263400977021;0.0025428814125209;1.79071885338914;0.002542896272539]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155980e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.140362e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785395759505415;-9.97184705328127;-6.6571492964578]
Robot Arm 1 Joint Angles: [-0.00246154399244598;2.14875822206857;-0.00246154399244025;-0.00729489265014786;-0.00246154399245873;-2.06288628749528;-0.00246154327944803]
Mobile Platform 2 Pose: [-2.34210809365107;9.97259522288809;6.41231466699192]
Robot Arm 2 Joint Angles: [0.00254290082915621;-2.09858702736374;0.00254289606457104;0.00569263400976591;0.00254288141252548;2.06919411153522;0.00254289627254358]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302894e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.521444e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362257766344;-6.64808091524349]
Robot Arm 1 Joint Angles: [-0.00246154399244884;1.86467432092128;-0.00246154399244313;-0.00729489265014114;-0.00246154399246161;-1.778802386348;-0.0024615432794509]
Mobile Platform 2 Pose: [-2.34210809365107;9.97371239780161;6.41426138376266]
Robot Arm 2 Joint Angles: [0.00254290082915892;-1.91983904120013;0.00254289606457375;0.00569263400976351;0.00254288141252819;1.89044612537161;0.00254289627254629]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531498e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446657e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.785395759505415;-9.97171662409789;-6.6579385588234]
Robot Arm 1 Joint Angles: [-0.00246154399245068;2.16962689140944;-0.00246154399244495;-0.00729489265013662;-0.00246154399246343;-2.08375495683617;-0.00246154327945273]
Mobile Platform 2 Pose: [-2.34210809365107;9.97366594781787;6.41395730063067]
Robot Arm 2 Joint Angles: [0.00254290082916057;-1.92727103859936;0.0025428960645754;0.005692634009762;0.00254288141252985;1.89787812277085;0.00254289627254794]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.613167e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.280457e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785395759505415;-9.97330001676718;-6.64968226679352]
Robot Arm 1 Joint Angles: [-0.00246154399245173;1.91628406432255;-0.00246154399244602;-0.00729489265013417;-0.0024615439924645;-1.83041212974928;-0.0024615432794538]
Mobile Platform 2 Pose: [-2.34210809365107;9.97263202256022;6.4124911995611]
Robot Arm 2 Joint Angles: [0.0025429008291616;-2.09269907982205;0.00254289606457643;0.00569263400976102;0.00254288141253087;2.06330616399353;0.00254289627254897]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408568e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.794592e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785395759505415;-9.97255494590405;-6.6537371298561]
Robot Arm 1 Joint Angles: [-0.00246154399245241;2.03549540242296;-0.00246154399244669;-0.00729489265013252;-0.00246154399246517;-1.9496234678497;-0.00246154327945446]
Mobile Platform 2 Pose: [-2.34210809365107;9.97432894735254;6.41512185482933]
Robot Arm 2 Joint Angles: [0.00254290082916219;-1.82119111305168;0.00254289606457702;0.00569263400976056;0.00254288141253146;1.79179819722317;0.00254289627254955]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394164e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.099202e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785395759505415;-9.97234650754405;-6.65460270477929]
Robot Arm 1 Joint Angles: [-0.00246154399245282;2.06884554002401;-0.0024615439924471;-0.00729489265013153;-0.00246154399246557;-1.98297360545075;-0.00246154327945487]
Mobile Platform 2 Pose: [-2.34210809365107;9.97240897095139;6.41200705174368]
Robot Arm 2 Joint Angles: [0.00254290082916258;-2.12838733723523;0.00254289606457741;0.00569263400976012;0.00254288141253185;2.09899442140672;0.00254289627254995]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956357e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.353832e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785395759505415;-9.97343026926908;-6.64918949930142]
Robot Arm 1 Joint Angles: [-0.00246154399245304;1.89544366401808;-0.00246154399244733;-0.00729489265013102;-0.00246154399246581;-1.80957172944482;-0.00246154327945511]
Mobile Platform 2 Pose: [-2.34210809365107;9.97398379544722;6.41464002913821]
Robot Arm 2 Joint Angles: [0.00254290082916279;-1.87641541790224;0.00254289606457762;0.00569263400976;0.00254288141253206;1.84702250207373;0.00254289627255016]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260469e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.096677e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164968951921;-6.65826235962286]
Robot Arm 1 Joint Angles: [-0.00246154399245322;2.18033642399812;-0.0024615439924475;-0.00729489265013054;-0.00246154399246597;-2.09446448942486;-0.00246154327945527]
Mobile Platform 2 Pose: [-2.34210809365107;9.97334388762253;6.41338656310922]
Robot Arm 2 Joint Angles: [0.00254290082916293;-1.97880066985296;0.00254289606457776;0.00569263400975984;0.0025428814125322;1.94940775402445;0.0025428962725503]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807923e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.758689e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785395759505415;-9.97358545598195;-6.64829065134618]
Robot Arm 1 Joint Angles: [-0.00246154399245328;1.87061378995892;-0.00246154399244758;-0.00729489265013045;-0.00246154399246605;-1.78474185538566;-0.00246154327945535]
Mobile Platform 2 Pose: [-2.34210809365107;9.97291257181965;6.41290482233546]
Robot Arm 2 Joint Angles: [0.00254290082916302;-2.04781119831344;0.00254289606457785;0.00569263400975974;0.00254288141253229;2.01841828248493;0.00254289627255039]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620323e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.152196e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.785395759505415;-9.97200013539397;-6.65659579684792]
Robot Arm 1 Joint Angles: [-0.00246154399245336;2.12426508403601;-0.00246154399244764;-0.00729489265013021;-0.00246154399246612;-2.03839314946275;-0.00246154327945542]
Mobile Platform 2 Pose: [-2.34210809365107;9.9742348232844;6.41487999062988]
Robot Arm 2 Joint Angles: [0.00254290082916306;-1.83625096395413;0.00254289606457789;0.00569263400975976;0.00254288141253233;1.80685804812562;0.00254289627255042]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640783e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.567062e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785395759505415;-9.97292968713781;-6.65164343752433]
Robot Arm 1 Joint Angles: [-0.00246154399245338;1.9755368050224;-0.00246154399244767;-0.00729489265013019;-0.00246154399246615;-1.88966487044914;-0.00246154327945544]
Mobile Platform 2 Pose: [-2.34210809365107;9.97233097336836;6.41187222254709]
Robot Arm 2 Joint Angles: [0.0025429008291631;-2.14086695052076;0.00254289606457793;0.00569263400975964;0.00254288141253238;2.11147403469225;0.00254289627255047]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711110e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.989371e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785395759505415;-9.97299183541534;-6.65154595274172]
Robot Arm 1 Joint Angles: [-0.0024615439924534;1.96559308061636;-0.00246154399244769;-0.00729489265013015;-0.00246154399246617;-1.8797211460431;-0.00246154327945546]
Mobile Platform 2 Pose: [-2.34210809365107;9.97418654828305;6.41490111408541]
Robot Arm 2 Joint Angles: [0.00254290082916311;-1.84397496417009;0.00254289606457794;0.00569263400975971;0.00254288141253238;1.81458204834159;0.00254289627255047]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373557e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.265689e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785395759505415;-9.97194709741018;-6.65673575840169]
Robot Arm 1 Joint Angles: [-0.00246154399245343;2.13275116144264;-0.00246154399244771;-0.00729489265013005;-0.00246154399246618;-2.04687922686938;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.34210809365107;9.97301384673674;6.41282183682782]
Robot Arm 2 Joint Angles: [0.00254290082916313;-2.03160721157977;0.00254289606457796;0.00569263400975965;0.0025428814125324;2.00221429575126;0.00254289627255049]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767804e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.486054e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360123983821;-6.64831637142007]
Robot Arm 1 Joint Angles: [-0.00246154399245341;1.86808837295744;-0.00246154399244771;-0.00729489265013014;-0.00246154399246618;-1.78221643838418;-0.00246154327945548]
Mobile Platform 2 Pose: [-2.34210809365107;9.97323694158299;6.413382023736]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.99591203617942;0.00254289606457796;0.00569263400975965;0.00254288141253241;1.96651912035092;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002526e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.098874e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166632921239;-6.65828886791101]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17767407308892;-0.00246154399244772;-0.00729489265013;-0.0024615439924662;-2.09180213851566;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34210809365107;9.97405718165475;6.41450618130501]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.86467362469809;0.00254289606457796;0.00569263400975969;0.0025428814125324;1.83528070886958;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.187228e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.310187e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785395759505415;-9.97339148554344;-6.64932526492237]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90164906012155;-0.00246154399244771;-0.00729489265013011;-0.00246154399246619;-1.81577712554829;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34210809365107;9.97237133511745;6.41192141220419]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.13440907066498;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10501615483647;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546799e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.844512e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785395759505415;-9.97241328591609;-6.65456938368534]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.05816100049619;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.97228906592293;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34210809365107;9.97430814871108;6.41502288903064]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82451889568434;0.00254289606457796;0.0056926340097597;0.0025428814125324;1.79512597985584;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.136121e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.367668e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724870348897;-6.65398600376929]
Robot Arm 1 Joint Angles: [-0.00246154399245344;2.04636116471989;-0.00246154399244772;-0.00729489265013004;-0.0024615439924662;-1.96048923014663;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34210809365107;9.97271503060728;6.41232260609211]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.07941779229304;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.05002487646453;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235454e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602043e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785395759505415;-9.97334546906772;-6.64975107323676]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.90901169623616;-0.00246154399244772;-0.00729489265013009;-0.00246154399246619;-1.8231397616629;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34210809365107;9.9735656399338;6.41386076051232]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.94332030004959;0.00254289606457797;0.00569263400975966;0.00254288141253241;1.91392738422108;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.018100e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.082652e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785395759505415;-9.97169001677896;-6.65815098765241]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.17388406243769;-0.00246154399244773;-0.00729489265012999;-0.0024615439924662;-2.08801212786443;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34210809365107;9.97380590496604;6.4140239680678]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.90487789489096;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.87548497906245;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924401e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.099282e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361436263305;-6.64826276794917]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.86598872578265;-0.00246154399244772;-0.00729489265013011;-0.00246154399246619;-1.78011679120939;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34210809365107;9.97252482683827;6.4121436689759]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.10985039533418;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.08045747950568;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213735e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.274217e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.785395759505415;-9.97189296008914;-6.6572402648311]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.14141313280842;-0.00246154399244773;-0.00729489265013;-0.0024615439924662;-2.05554119823516;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34210809365107;9.9743431422198;6.41499643236305]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.81891993429021;0.00254289606457796;0.0056926340097597;0.00254288141253241;1.78952701846171;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673361e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.746296e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.780060009631965;-9.97305746965453;-6.65109569871791]
Robot Arm 1 Joint Angles: [-0.00246154585509858;1.95597189978402;-0.00246154585509287;-0.00729489078748492;-0.00246154585511135;-1.86921966963569;-0.00246154514210064]
Mobile Platform 2 Pose: [-2.34210809365107;9.97248506707512;6.41194299756552]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.11621195743906;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.08681904161055;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.181800e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.588938e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.780060009631965;-9.97285991725256;-6.65233913585984]
Robot Arm 1 Joint Angles: [-0.00246154585509859;1.98758028410009;-0.00246154585509287;-0.00729489078748491;-0.00246154585511135;-1.90082805395175;-0.00246154514210065]
Mobile Platform 2 Pose: [-2.34210809365107;9.97386243168368;6.41428302761475]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.89583362006872;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.86644070424021;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.352451e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.362864e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.77402105973086;-9.97206302236907;-6.65624204481392]
Robot Arm 1 Joint Angles: [-0.00246154399245345;2.0943951023932;-0.00246154399244773;-0.00729489265013001;-0.0024615439924662;-2.02833123344619;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34210809365107;9.97350003033442;6.41347027360621]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.95381783594978;0.00254289606457797;0.00569263400975965;0.00254288141253241;1.92442492012127;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.853149e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.671985e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.77402105973086;-9.97356375106397;-6.64863608494535]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.85427851120923;-0.00246154399244772;-0.0072948926501301;-0.00246154399246619;-1.78821464226223;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34210809365107;9.97277160699723;6.41250687645281]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.07036556990124;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.04097265407273;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.226598e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.796764e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.77402105973086;-9.97163361847856;-6.65855413216156]
Robot Arm 1 Joint Angles: [-0.00246154399245346;2.16309972487513;-0.00246154399244773;-0.00729489265012998;-0.0024615439924662;-2.09703585592812;-0.0024615432794555]
Mobile Platform 2 Pose: [-2.34210809365107;9.97429026720412;6.41482222058183]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82737993679863;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.79798702097012;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.111579e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.466001e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.77402105973086;-9.97344030356764;-6.64918795446899]
Robot Arm 1 Joint Angles: [-0.00246154399245343;1.87403011062256;-0.00246154399244772;-0.0072948926501301;-0.00246154399246619;-1.80796624167555;-0.00246154327945549]
Mobile Platform 2 Pose: [-2.34210809365107;9.97235351915643;6.41172343005482]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13725962442818;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10786670859967;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.041297e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.233112e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.76857278166737;-9.97233227648874;-6.65509212776466]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.05131444138422;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.98525057243721;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97409988032161;6.41460354864164]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.8578418380005;0.00254289606457796;0.00569263400975968;0.00254288141253241;1.82844892217199;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.708081e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.756456e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.76857278166737;-9.97253405647212;-6.65385333675952]
Robot Arm 1 Joint Angles: [-0.00246154212980829;2.01902964404354;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.95296577509653;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97316907511273;6.41289533507412]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.00677067142089;0.00254289606457797;0.00569263400975964;0.00254288141253242;1.97737775559238;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.579506e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.760918e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.76857278166737;-9.9733175648311;-6.65000946836689]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.89366830660643;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.82760443765942;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97308002325655;6.41296146892642]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.02101896840894;0.00254289606457797;0.00569263400975963;0.00254288141253242;1.99162605258043;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.464144e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.431962e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.76857278166737;-9.97168790275993;-6.65827209487266]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.15441423799437;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.08835036904736;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97415148244386;6.41450883936743]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.84958549843966;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.82019258261116;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.705149e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.538237e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.76857278166737;-9.97361102787369;-6.648391133195]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84671421979171;-0.00246154212980257;-0.00729489451277526;-0.00246154212982105;-1.7806503508447;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.9723374048792;6.4116852718328]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13983790878523;0.00254289606457798;0.00569263400975959;0.00254288141253242;2.11044499295673;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.172686e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.026317e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.76857278166737;-9.97190223563353;-6.65730673598685]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.1201209782182;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.05405710927119;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97426103134744;6.41479325047432]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.83205767386667;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.80266475803816;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.545159e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.751568e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.76857278166737;-9.97301256220243;-6.65143728462743]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.94246872719313;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.87640485824612;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97285048540522;6.41235772036275]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.05774502462155;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.02835210879305;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.638023e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.726534e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.76857278166737;-9.97291306627382;-6.65217502462558]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.9583880757713;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.89232420682429;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97341154161144;6.41344719277614]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.96797603162664;0.00254289606457797;0.00569263400975965;0.00254288141253241;1.93858311579813;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.075416e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.044794e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.76857278166737;-9.97198492257506;-6.65675907697899]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.10689106757347;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.04082719862646;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97393341513048;6.41407435068344]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.88447626858118;0.00254289606457797;0.00569263400975968;0.00254288141253241;1.85508335275267;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.286978e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.778851e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.76857278166737;-9.97358975797768;-6.64860173182345]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.8501174031534;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.78405353420639;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97243881475882;6.41182813639948]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.12361232804653;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.09421941221802;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.611243e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.036839e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.76857278166737;-9.97165344350814;-6.65857441737706]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.1599277182802;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09386384933319;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97433749811517;6.41483760929609]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.81982299103073;0.00254289606457796;0.0056926340097597;0.0025428814125324;1.79043007520222;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.966213e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.356078e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.76857278166737;-9.97338687584304;-6.64956990354873]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.88257854469673;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.81651467574972;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97258353266907;6.41191529615376]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.10045746240563;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.07106454657712;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.326307e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.112662e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.76857278166737;-9.97242533855192;-6.65472926852525]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.03642451127528;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.97036064232827;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97372752370028;6.41390283329569]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.91741889741171;0.00254289606457797;0.00569263400975966;0.00254288141253241;1.8880259815832;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.610994e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.794992e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.76857278166737;-9.97243963023017;-6.65445145721341]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.03413784275479;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.96807397380778;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97365041560225;6.41354909374253]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.9297561930974;0.00254289606457797;0.00569263400975966;0.00254288141253241;1.9003632772689;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.002963e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.255926e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.76857278166737;-9.97337820896761;-6.64979791904135]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.88396524476524;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.81790137581823;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97264461995614;6.41212965936704]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.0906834964739;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.06129058064539;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.906709e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.092592e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.76857278166737;-9.97165729113912;-6.65854870599695]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.15931209732387;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09324822837686;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97432629404556;6.41473296726721]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82161564216786;0.00254289606457796;0.00569263400975969;0.0025428814125324;1.79222272633936;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.140997e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.325142e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.76857278166737;-9.97359310489233;-6.64858598821476]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84958189680944;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.78351802786243;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97240229994916;6.41161583833454]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.12945469759137;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10006178176286;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.282823e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.781205e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.76857278166737;-9.97197355581467;-6.65706107294213]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.10870974923445;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.04264588028744;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97399588312742;6.41427627440907]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.87448138907057;0.00254289606457796;0.00569263400975968;0.00254288141253241;1.84508847324206;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.812330e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.271109e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.76857278166737;-9.97292647968527;-6.65199043047942]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.95624192993971;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.8901780609927;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97332707838788;6.41297727896475]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.98149014739665;0.00254289606457797;0.00569263400975964;0.00254288141253241;1.95209723156815;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.216729e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.718747e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.76857278166737;-9.97299973670691;-6.65183558305077]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.94452080647721;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.8784569375302;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97292829600278;6.41254791735673]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.04529532901218;0.00254289606457797;0.00569263400975962;0.00254288141253242;2.01590241318367;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.647654e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.357807e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.76857278166737;-9.97191267600682;-6.65724629797725]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.11845051849125;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.05238664954424;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97422781375111;6.41448404274056]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.83737248927871;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.8079795734502;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.092956e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.556004e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.76857278166737;-9.9736087876097;-6.64860323618479]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84707266203059;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.78100879308358;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97233018106088;6.41149024259799]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.14099371971526;0.00254289606457798;0.00569263400975959;0.00254288141253242;2.11160080388675;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.225461e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.380511e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.76857278166737;-9.97168269309921;-6.65854616739385]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.15524778370881;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.0891839147618;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97419474857498;6.4145306952303]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.84266291745975;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.81327000163125;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.267928e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.340049e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.76857278166737;-9.97332718940466;-6.64998442909648]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.89212837483686;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.82606450588985;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97299767053238;6.41241446045273]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.03419540427643;0.00254289606457797;0.00569263400975963;0.00254288141253242;2.00480248844793;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.240979e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.986407e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.76857278166737;-9.9725197089022;-6.65435826042663]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.0213252552299;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.9552613862829;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97325382410881;6.41302667825918]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.99321083204769;0.00254289606457797;0.00569263400975964;0.00254288141253241;1.96381791621918;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.119166e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.083889e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.76857278166737;-9.97234632998976;-6.65504420440185]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.04906588122068;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.98300201227367;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97404615007965;6.41410408979979]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.86643867671355;0.00254289606457796;0.00569263400975968;0.00254288141253241;1.83704576088504;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.484908e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.856852e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.76857278166737;-9.97343265756008;-6.64961760163626]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.87525346996959;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.80918960102258;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97237651982892;6.41154863407448]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13357951682959;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10418660100108;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.845434e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.106544e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.76857278166737;-9.97163605432937;-6.65877775702865]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.1627099868826;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09664611793559;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97431205004789;6.41464508915858]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82389468179417;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.79450176596566;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.529755e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.167477e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.76857278166737;-9.97356818540841;-6.64881763762023]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.85356901423618;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.78750514528917;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97270148767499;6.41192008142869]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.08158466145857;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.05219174563007;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.473485e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.860221e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.76857278166737;-9.97205088347434;-6.65678381261084]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.09633732368672;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.03027345473971;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97358171106066;6.41350385531709]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.94074891975059;0.00254289606457797;0.00569263400975966;0.00254288141253241;1.91135600392208;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.316507e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.095680e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.76857278166737;-9.972837092466;-6.65256098942493]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.97054388502172;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.90448001607472;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97379157625095;6.41361727086619]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.90717048930408;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.87777757347557;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.546441e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.473369e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.76857278166737;-9.97308256872618;-6.65151446423875]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.93126768339328;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.86520381444627;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97253531296617;6.41177894464269]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.10817261486956;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.07877969904105;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.753799e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.595125e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.76857278166737;-9.97184705841051;-6.65770006752504]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.12894933389963;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.06288546495262;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97434260400427;6.41461108215871]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.81900604877317;0.00254289606457796;0.0056926340097597;0.00254288141253241;1.78961313294467;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.541401e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.484093e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.76857278166737;-9.97362078825901;-6.64864131708292]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84515255814036;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77908868919335;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97247588762365;6.41154774630312]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.1176806696729;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.0882877538444;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.675049e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.126858e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.76857278166737;-9.97172105943777;-6.65847115380272]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.14910916953786;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08304530059085;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97387606351977;6.41392191835062]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.89365252629351;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.864259610465;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.336605e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.178691e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.76857278166737;-9.97326153141652;-6.65043031976704]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.90263365293837;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.83656978399136;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97348365229618;6.41306121540103]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.95643832206742;0.00254289606457797;0.00569263400975965;0.00254288141253241;1.92704540623891;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.815614e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.994592e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.76857278166737;-9.9726144560741;-6.65398381637337]
Robot Arm 1 Joint Angles: [-0.00246154212980829;2.00616570772535;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.94010183877834;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97278603949327;6.4121480665893]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.06805637053294;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.03866345470444;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.830572e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.625418e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.76857278166737;-9.97225509777386;-6.65562676301768]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.06366303576333;-0.00246154212980257;-0.00729489451277517;-0.00246154212982105;-1.99759916681632;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97428531466857;6.41442956809892]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82817234248551;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.798779426657;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.266138e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.958154e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.76857278166737;-9.97348066524364;-6.6494699648377]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.86757224059869;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.80150837165168;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97234988545577;6.41133701032079]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13784101653289;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10844810070438;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.113260e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.600966e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.76857278166737;-9.97162442629164;-6.65895800368076]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.16457047291865;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09850660397164;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97410997331452;6.41423638153226]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.85622695913399;0.00254289606457796;0.00569263400975968;0.00254288141253241;1.82683404330548;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.045442e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.213840e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.76857278166737;-9.97353633215374;-6.64908576912618]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.85866553498285;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.79260166603584;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97315239060475;6.41248673993037]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.00944019269561;0.00254289606457797;0.00569263400975964;0.00254288141253242;1.9800472768671;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.321437e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.109192e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.76857278166737;-9.97213339927729;-6.65647900513482]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.08313479521536;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.01707092626835;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97309656594408;6.41260570928861]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.01837213840271;0.00254289606457797;0.00569263400975963;0.00254288141253242;1.9889792225742;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.769639e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.066853e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.76857278166737;-9.97274513514851;-6.65314516130932]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.98525705582042;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.91919318687342;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97414232255858;6.41410955795973]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.85105108008336;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.82165816425485;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.480466e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782451e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.76857278166737;-9.97316095527034;-6.65121489729356]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.91872583632685;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.85266196737984;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97233978357517;6.41130818550213]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13945731742877;0.00254289606457798;0.00569263400975959;0.00254288141253242;2.11006440160026;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.183435e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.477417e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.76857278166737;-9.97178877727758;-6.65811677362549]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.1382743151678;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.07221044622079;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.9742669881683;6.41441898151681]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.83110458252762;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.80171166669911;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.796983e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.062574e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.76857278166737;-9.97362572769691;-6.64871625866114]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84436224807658;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77829837912957;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97283549256625;6.41195260783918]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.06014387885714;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.03075096302863;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.225710e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.913792e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.76857278166737;-9.97176813647359;-6.65835132418411]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.14157684380704;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.07551297486004;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97342818409695;6.41309132476823]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.96531323394475;0.00254289606457797;0.00569263400975965;0.00254288141253241;1.93592031811624;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.585614e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.651107e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.76857278166737;-9.9731902541302;-6.65090571154465]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.91403801874958;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.84797414980257;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97392056454949;6.41366966183132]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.88653236153844;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.85713944570994;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.830775e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.028934e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.76857278166737;-9.97270867125007;-6.65361056039674]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.99109127957075;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.92502741062374;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97244689312227;6.41145975972259]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.12231978989385;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.09292687406535;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.106067e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.651741e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.76857278166737;-9.97216687363648;-6.65619433372216]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.07777889774471;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.01171502879771;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.9743390693493;6.41445583355052]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.81957159356848;0.00254289606457796;0.0056926340097597;0.00254288141253241;1.79017867773997;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.854149e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.613623e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.76857278166737;-9.97352203832939;-6.64935619139666]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.86095254687899;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.79488867793199;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97257212618277;6.41151638130712]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.10228250021374;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.07288958438523;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602719e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.806761e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.76857278166737;-9.97162253803603;-6.65908871433669]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.16487259381631;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09880872486931;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97374244483633;6.41354393862708]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.91503151564378;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.88563859981527;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.426946e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.196680e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.76857278166737;-9.97349763610396;-6.6493899080495]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.86485690294713;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.79879303400012;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97363486215361;6.41314090129797]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.93224474487923;0.00254289606457797;0.00569263400975966;0.00254288141253241;1.90285182905072;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.576428e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.416757e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.76857278166737;-9.97222023331086;-6.65615096823033]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.06924134984443;-0.00246154212980257;-0.00729489451277517;-0.00246154212982105;-2.00317748089743;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97265735007124;6.4117683041215]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.08864667805707;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.05925376222857;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.419475e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.441840e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.76857278166737;-9.97265140625674;-6.65373882791478]
Robot Arm 1 Joint Angles: [-0.00246154212980829;2.0002536785025;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.93418980955549;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97432343231712;6.41434374243465]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.8220735187162;0.00254289606457796;0.0056926340097597;0.00254288141253241;1.79268060288769;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.723995e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.598878e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.76857278166737;-9.97323436387051;-6.65093975247866]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.90698046029935;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.84091659135235;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97239594564589;6.41122511599895]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13047138611345;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10107847028494;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.130671e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.288408e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.76857278166737;-9.97173846302947;-6.65849319149926]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.14632459486686;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.08026072591986;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97400772701446;6.41391210997034]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.87258636714223;0.00254289606457797;0.00569263400975968;0.00254288141253241;1.84319345131372;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.591685e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.474444e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.76857278166737;-9.97362359054118;-6.64882823774895]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84470419299342;-0.00246154212980257;-0.00729489451277526;-0.00246154212982104;-1.77864032404641;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97331034317805;6.4125681582664]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.98416778096765;0.00254289606457797;0.00569263400975964;0.00254288141253242;1.95477486513914;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.378531e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.878487e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.76857278166737;-9.97182342419144;-6.65818909812268]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.13273080895104;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.06666694000403;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97294404289009;6.4121910271142]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.04277582704178;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.01338291121328;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.812806e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.601108e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.76857278166737;-9.97311378040979;-6.65140837794644]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.92627381401541;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.8602099450684;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97422062938993;6.41408782923236]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.83852198706784;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.80912907123933;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.143962e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.702162e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.76857278166737;-9.97280148188551;-6.6532429597475]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.97624157789994;-0.00246154212980257;-0.0072948945127752;-0.00246154212982105;-1.91017770895294;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97232969746502;6.41110872946906]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.14107109505332;0.00254289606457798;0.00569263400975959;0.00254288141253242;2.11167817922481;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.855824e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.850672e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.76857278166737;-9.9720825810128;-6.65674220625662]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.09126571753406;-0.00246154212980258;-0.00729489451277516;-0.00246154212982105;-2.02520184858706;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97420269624267;6.41415985031195]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.84139129062828;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.81199837479978;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.941576e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.603808e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.76857278166737;-9.97355662740474;-6.64927723131845]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.85541829482389;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.78935442587688;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97298166639312;6.41200739046929]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.03675606655643;0.00254289606457798;0.00569263400975963;0.00254288141253242;2.00736315072792;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.243858e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.564000e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.76857278166737;-9.97163041609551;-6.65916967751948]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.16361210430033;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09754823535332;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97327062104862;6.41267117885745]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.99052332167644;0.00254289606457797;0.00569263400975964;0.00254288141253242;1.96113040584793;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.336803e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.291988e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.76857278166737;-9.97345222285591;-6.64972938681886]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.87212302263595;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.80605915368894;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97403499099565;6.41370182696881]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.8682241301513;0.00254289606457797;0.00569263400975968;0.00254288141253241;1.83883121432279;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.087531e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.497095e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.76857278166737;-9.97231047752147;-6.65580422759416]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.05480227614708;-0.00246154212980257;-0.00729489451277517;-0.00246154212982105;-1.98873840720008;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97238196522403;6.41117624167287]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13270825361135;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10331533778284;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.780616e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408750e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.76857278166737;-9.97255675911689;-6.65433760009701]
Robot Arm 1 Joint Angles: [-0.00246154212980829;2.01539722087946;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.94933335193245;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97431570966947;6.41426688296417]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82330914234107;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.79391622651256;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.691126e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.117438e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.76857278166737;-9.97330233617798;-6.65069153977302]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.89610489110503;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.83004102215803;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97268819869477;6.41151797458278]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.08371089829298;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.05431798246447;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.686451e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.161805e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.76857278166737;-9.97169666148579;-6.65882652024548]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.15301284185558;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.08694897290857;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97359760869745;6.41314665674718]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.93820529786417;0.00254289606457797;0.00569263400975966;0.00254288141253241;1.90881238203567;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.751837e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.322831e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.76857278166737;-9.97361437717986;-6.64897732868856]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84617833080486;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.78011446185785;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97377718736213;6.41321052297952]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.90947271151538;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.88007979568687;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.711843e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.916088e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.76857278166737;-9.97188633405845;-6.65798734190149]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.1226652302293;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.05660136128229;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97254597817422;6.41141447728111]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.1064661815808;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.07707326575229;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.651879e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.113597e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.76857278166737;-9.97303260756897;-6.65193571308903]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.93926146854697;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.87319759959997;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.974341845709;6.41422536934612]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.81912737601679;0.00254289606457796;0.0056926340097597;0.00254288141253241;1.78973446018828;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.661762e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.370989e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.76857278166737;-9.97289206390315;-6.65288526342758]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.96174845507858;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.89568458613157;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97246701279052;6.41115297452726]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.11910064297304;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.08970772714453;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.340764e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.325167e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.76857278166737;-9.97200311265445;-6.65726582934695]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.10398065486985;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.03791678592284;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97388946666322;6.41356042521078]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.89150802334139;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.86211510751288;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.343120e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.125191e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.76857278166737;-9.97358432012228;-6.6492338345256]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.85098746001703;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.78492359107002;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97346730194258;6.41265224934448]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.9590543786433;0.00254289606457797;0.00569263400975965;0.00254288141253241;1.92966146281479;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.907817e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.381239e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.76857278166737;-9.97164798209862;-6.65920120415237]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.16080154380273;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09473767485572;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97280054704856;6.41178935253054]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.06573516168652;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.03634224585801;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.097889e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.601532e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.76857278166737;-9.97340025980208;-6.65010331073404]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.88043711124835;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.81437324230134;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97428017123687;6.41403661536948]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82899529155677;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.79960237572826;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.623094e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.525447e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.76857278166737;-9.9724031993016;-6.65544345135017]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.03996679132576;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.97390292237876;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97234656784146;6.41095107380885]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.1383718348222;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10897891899369;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.636184e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023453e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.76857278166737;-9.97246209056899;-6.65493687548571]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.03054418854305;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.96448031959604;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97411981578766;6.41386879255311]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.85465216343075;0.00254289606457796;0.00569263400975968;0.00254288141253241;1.82525924760224;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.069124e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.749685e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.76857278166737;-9.97336448522108;-6.65047241845527]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.88616104420931;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.82009717526231;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97313583227842;6.41207838519346]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.01208952490891;0.00254289606457797;0.00569263400975964;0.00254288141253242;1.9826966090804;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.048977e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.474651e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.76857278166737;-9.97166382756818;-6.65911441338882]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.1582662686732;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09220239972619;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97311307272715;6.41224987343838]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.01573105311149;0.00254289606457797;0.00569263400975963;0.00254288141253242;1.98633813728299;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.164008e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.822148e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.76857278166737;-9.97359810429041;-6.64916350066282]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84878199311538;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.78271812416837;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97413301161772;6.41371005895604]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.85254083062095;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.82314791479244;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.608243e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.067106e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.76857278166737;-9.97195619571484;-6.65774933727887]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.11148736520715;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.04542349626014;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97234244884715;6.41093152757369]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.13903087391159;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10963795808308;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.880053e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023707e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.76857278166737;-9.97294730908925;-6.65248472533289]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.95290922530183;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.88684535635482;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97427269747554;6.41404429525368]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.83019109336944;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.80079817754093;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.279264e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.430264e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.76857278166737;-9.97297965195079;-6.65254144966833]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.94773436745455;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.88167049850754;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97282071719276;6.4115478642153]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.06250793861528;0.00254289606457798;0.00569263400975962;0.00254288141253242;2.03311502278678;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.806838e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.029333e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.76857278166737;-9.97192931752244;-6.65776087558479]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.11578787599151;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.0497240070445;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97344469150917;6.4127352248386]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.96267204798916;0.00254289606457797;0.00569263400975965;0.00254288141253241;1.93327913216065;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.452071e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.381222e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.76857278166737;-9.973605034599;-6.64922658096752]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84767314374192;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.78160927479491;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97390761996406;6.41326486294586]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.88860349520637;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.85921057937786;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.791598e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.206800e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.76857278166737;-9.97167505344599;-6.65918412462028]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.15647012822334;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09040625927633;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97245519146641;6.41109170410197]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.12099205483108;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.09159913900257;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.346569e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.365441e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.76857278166737;-9.97334196371328;-6.65051052188467]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.88976448545695;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.82370061650995;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97434041045208;6.41407367401235]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.81935701712439;0.00254289606457796;0.0056926340097597;0.0025428814125324;1.78996410129588;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.432537e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.921295e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.76857278166737;-9.97249745591252;-6.65507337998379]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.02488573357792;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.95882186463092;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97256100361438;6.41111792159858]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.10406211115614;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.07466919532763;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.364825e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.622167e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.76857278166737;-9.97236832795706;-6.65553190447206]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.04554620645218;-0.00246154212980257;-0.00729489451277518;-0.00246154212982105;-1.97948233750517;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.9737571597697;6.41318469749416]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.91267712630453;0.00254289606457797;0.00569263400975967;0.00254288141253241;1.88328421047602;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.884303e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.660258e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.76857278166737;-9.97342049076546;-6.65028421599521]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.87720015710803;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.81113628816102;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97361929302393;6.41273273179038]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.93473580562768;0.00254289606457797;0.00569263400975966;0.00254288141253241;1.90534288979918;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.936596e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.799087e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.76857278166737;-9.97164032017515;-6.65935500368314]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.16202745155694;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.09596358260993;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97267020550757;6.41140712247345]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.0865898082447;0.00254289606457798;0.00569263400975961;0.00254288141253242;2.0571968924162;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.603272e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.851394e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.76857278166737;-9.97357480694426;-6.64938660752502]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.8525095685001;-0.00246154212980257;-0.00729489451277525;-0.00246154212982105;-1.7864456995531;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97432036601747;6.41395418741638]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.82256412666114;0.00254289606457796;0.00569263400975969;0.0025428814125324;1.79317121083263;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673283e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.930817e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.76857278166737;-9.97203226498885;-6.6574787438835]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.09931628136552;-0.00246154212980258;-0.00729489451277515;-0.00246154212982105;-2.03325241241851;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97238990560285;6.41083488012112]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.13143779300088;0.00254289606457798;0.0056926340097596;0.00254288141253242;2.10204487717237;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.359401e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.890724e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.76857278166737;-9.97285853389263;-6.65305204308413]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.96711325676109;-0.00246154212980257;-0.00729489451277521;-0.00246154212982105;-1.90104938781409;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97401932795831;6.41354753723369]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.87073021612735;0.00254289606457796;0.00569263400975968;0.00254288141253241;1.84133730029884;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.218230e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.711618e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.76857278166737;-9.97306354733001;-6.65221518433034]
Robot Arm 1 Joint Angles: [-0.00246154212980829;1.93431110678034;-0.00246154212980257;-0.00729489451277522;-0.00246154212982105;-1.86824723783333;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97329368714426;6.4121592082592]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.98683274637458;0.00254289606457797;0.00569263400975964;0.00254288141253241;1.95743983054607;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.851118e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.280694e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.76857278166737;-9.97186198888549;-6.65822330010249]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.12656045790309;-0.00246154212980258;-0.00729489451277514;-0.00246154212982105;-2.06049658895608;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97295980573149;6.41183414126615]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.0402537724178;0.00254289606457797;0.00569263400975962;0.00254288141253242;2.01086085658929;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.771672e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.889167e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.76857278166737;-9.97361871377409;-6.64925590648609]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.84548447572642;-0.00246154212980257;-0.00729489451277525;-0.00246154212982104;-1.77942060677941;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.9742132744683;6.41369135783897]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.8396987745286;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.8103058587001;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.982678e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.003149e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.76857278166737;-9.9717113450934;-6.65911978064493]
Robot Arm 1 Joint Angles: [-0.00246154212980831;2.15066346463683;-0.00246154212980258;-0.00729489451277513;-0.00246154212982105;-2.08459959568982;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97232951782174;6.41072767532274]
Robot Arm 2 Joint Angles: [0.00254290082916315;-2.14109983797886;0.00254289606457798;0.00569263400975959;0.00254288141253242;2.11170692215035;0.00254289627255052]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.814685e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.416229e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.76857278166737;-9.97327760827044;-6.65094956348835]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.90006135631116;-0.00246154212980257;-0.00729489451277523;-0.00246154212982105;-1.83399748736416;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97421039370452;6.41378858344467]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.84015969673417;0.00254289606457796;0.00569263400975969;0.00254288141253241;1.81076678090566;0.0025428962725505]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.939219e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.009145e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.76857278166737;-9.97259230930518;-6.65469875383098]
Robot Arm 1 Joint Angles: [-0.00246154212980829;2.00970919075211;-0.00246154212980257;-0.00729489451277519;-0.00246154212982105;-1.9436453218051;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97296583801377;6.41160063147371]
Robot Arm 2 Joint Angles: [0.00254290082916314;-2.03928860725424;0.00254289606457797;0.00569263400975962;0.00254288141253242;2.00989569142573;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.985859e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.231303e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.76857278166737;-9.97227641493954;-6.65611786175559]
Robot Arm 1 Joint Angles: [-0.0024615421298083;2.06025228925576;-0.00246154212980258;-0.00729489451277517;-0.00246154212982105;-1.99418842030875;-0.00246154141681035]
Mobile Platform 2 Pose: [-2.34210809365107;9.97328732734791;6.41231551773757]
Robot Arm 2 Joint Angles: [0.00254290082916314;-1.98785031379091;0.00254289606457797;0.00569263400975964;0.00254288141253241;1.95845739796241;0.00254289627255051]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.511170e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.541017e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.76857278166737;-9.97347009323699;-6.65012845411343]
Robot Arm 1 Joint Angles: [-0.00246154212980828;1.86926376166388;-0.00246154212980257;-0.00729489451277524;-0.00246154212982105;-1.80319989271687;-0.00246154141681034]
Mobile Platform 2 Pose: [-2.34210809365107;9.97402370926079;6.41329940114241]
Robot Arm 2 Joint Angles: [0.00254290082916313;-1.87002920773047;0.00254289606457796;0.00569263400975968;0.00254288141253241;1.84063629190196;0.0025428962725505]
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.632661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mat2str')" style="font-weight:bold">mat2str</a>
输入矩阵必须是数值数组、字符数组或字符串数组。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 99)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',99,0)">第 99 行</a>)
    fprintf('Robot Arm 1 end: %s\n', mat2str(mobile_robot1.arm.joint_world));
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.604479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 end: [0.999122851379701 -0.0132271878271526 -0.0397312138385112 -9.8543202540212;0.0132183254048484 0.999912516130377 -0.0004857564306706 -9.82617087977973;0.0397341631897264 -3.9849763159419e-05 0.999210285519324 1.50722002620516;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629097324578;9.99866771831298;9.81844391176629]
Robot Arm 2 end: [0.998790100872192 0.0406335882993141 0.0276992039892478 9.85353116099417;-0.0406191175090511 0.999174113853955 -0.00108512531554088 9.82440257157176;-0.0277204201357414 -4.13047983783923e-05 0.999615714463019 1.50725286605147;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.085599e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 end: [0.996738002951666 -0.0127721274666221 -0.0796883067451046 -9.72929347716611;0.0127439626034678 0.999918420833346 -0.000862029761223302 -9.65232574912907;0.0796928157934387 -0.000156326978406671 0.996819457410814 1.40200599186901;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629085493335;9.99733167915688;9.63690091788906]
Robot Arm 2 end: [0.997632300480546 0.0401784874653565 0.0558165045742719 9.72764003270172;-0.0401248114767599 0.99919250541078 -0.00208245885519979 9.64801727527021;-0.0558551030958499 -0.000162098504965317 0.998438872030845 1.40568255465579;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.756819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 end: [0.992712630059011 -0.0123665703335376 -0.119869520978049 -9.69930061746444;0.012318179458645 0.999923473158929 -0.00114467489254313 -9.47951608220888;0.119874503444836 -0.000340241047890415 0.992788994529997 1.23897968990596;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629073661568;9.99599115889047;9.45545426170748]
Robot Arm 2 end: [0.995634726925002 0.039771810706594 0.0844375130738446 9.69154744118808;-0.0396594186932118 0.999208726106999 -0.00300868418829441 9.46812272473028;-0.084490360692172 -0.00035319222419307 0.996424234051621 1.2488728630185;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.583794e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 end: [0.986925364740101 -0.0120327893135965 -0.160728144436355 -9.77867309312711;0.0119689596995511 0.999927437312061 -0.00136532569196871 -9.30834404725646;0.160732910246563 -0.000576274126807119 0.986997770753207 1.12386821017013;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629061848555;9.99464273526989;9.27450988275633]
Robot Arm 2 end: [0.992689368902542 0.0394351091115981 0.114073174047317 9.75805533520809;-0.0392457611942277 0.999221953776981 -0.00390606175047757 9.28464215800666;-0.114138455816429 -0.000599382573352645 0.993464671563298 1.13383266273811;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.541740e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 end: [0.979172775485781 -0.0117887649789589 -0.202686212574373 -9.85305702661775;0.0117151049888496 0.999930153993491 -0.00156315375249349 -9.13781278677307;0.202690483404064 -0.000843892661760998 0.979242490797148 1.12021872025027;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629050071916;9.99328288888451;9.0945117500114]
Robot Arm 2 end: [0.988620683271488 0.0391858066285229 0.145236418183211 9.82638678283508;-0.0388977082655136 0.999231553567938 -0.00482396578522646 9.10209881739863;-0.145313842766295 -0.000880291473349828 0.98938521930915 1.1181253975181;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.613259e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346623]
Robot Arm 1 end: [0.969358512351231 -0.0116458204980189 -0.245374100909483 -9.83163316704073;0.011564525338854 0.999931558185561 -0.00177220153554094 -8.96674749629692;0.24537794580177 -0.00111973636338769 0.969426846082045 1.10413295360329;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629038312944;9.99191235976656;8.91540930204226]
Robot Arm 2 end: [0.983301596090842 0.039035055761688 0.177747673815678 9.81758164879397;-0.0386212218665538 0.999237153785995 -0.00578893040194062 8.92353966067053;-0.1778380508967 -0.00117256784281213 0.984059069740184 1.09909329154178;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528709;-9.99002831581144;-8.79949883355178]
Robot Arm 1 end: [0.957536723416997 -0.0116087621521956 -0.288077524199886 -9.88437209684222;0.011513895345975 0.999931665029773 -0.00202373040398639 -8.79686073268705;0.288081331435767 -0.00137909828505258 0.957604952246027 0.993019402149945;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629026528254;9.99053428373392;8.73682083139294]
Robot Arm 2 end: [0.976698721884454 0.0389878013617696 0.211043971755294 9.86446578356857;-0.0384150360866669 0.999238632614992 -0.00681469677833056 8.74321910142488;-0.211148979802731 -0.00145135615742755 0.977452813128885 1.01042331102585;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274173]
Robot Arm 1 end: [0.943511938731262 -0.0116779859632009 -0.331132671470845 -10.0240635535027;0.0115472202145814 0.999930538209914 -0.00236229984284014 -8.62844670979544;0.331137257307135 -0.00159480377313383 0.94358124897861 1.06365451935044;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629014716292;9.98914654796114;8.55900550587845]
Robot Arm 2 end: [0.968638432608659 0.039044449620499 0.24538768882581 9.96949460581144;-0.0382639656686109 0.999236046534113 -0.00794935457678013 8.56148628211128;-0.245510602224734 -0.0016894557432278 0.969392433400705 1.05871774249676;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196129;-9.98711511781;-8.46062525935787]
Robot Arm 1 end: [0.927245027302788 -0.0118469413494187 -0.374267697407773 -9.83059699093514;0.0116373715365835 0.99992830730663 -0.00281989904398087 -8.4567607557185;0.37427427232709 -0.00174075488284873 0.927316417867421 1.11116984223786;0 0 0 1]
Mobile Platform 2 Pose: [-2.35629002867934;9.98774923952431;8.38210547777925]
Robot Arm 2 end: [0.959000040225796 0.0391988979387902 0.280681971717646 9.84188315161445;-0.0381435224851084 0.999229696339848 -0.00922418804035626 8.38983329068779;-0.280827339373055 -0.00186020239762461 0.959756503029644 1.07398937608745;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929674;-9.98565931998688;-8.29072857539468]
Robot Arm 1 end: [0.972599318669045 -0.0120247397034109 -0.232176594340547 -9.9514182120527;0.012164199416326 0.999925668029231 -0.000831064892159842 -8.2888018474312;0.232169329535728 -0.00201594924547742 0.972673294776602 0.775243010355489;0 0 0 1]
Mobile Platform 2 Pose: [-2.35628990890741;9.98635048644342;8.20503341586822]
Robot Arm 2 end: [0.97153982954801 0.0394088205033351 0.233575051040045 9.97772933106647;-0.0387943494160902 0.999221086618418 -0.00722623764414399 8.20703280402994;-0.233677893809419 -0.00204081445586356 0.972311923726718 0.847078554133814;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.9841823282427;-8.12462828169497]
Robot Arm 1 end: [0.966888868660408 -0.0123151613798642 -0.25490047559937 -10.2177197802578;0.0124206796639061 0.99992214547289 -0.00119570463669971 -8.1250548188438;0.254895355738946 -0.00200992365016651 0.966966554659279 1.3488083098789;0 0 0 1]
Mobile Platform 2 Pose: [-2.35628978868132;9.98493699942165;8.03016607340824]
Robot Arm 2 end: [0.967605020813912 0.0397003935626714 0.249327901460464 10.0962612391117;-0.0389550281981147 0.999209537425857 -0.00792502967122362 8.02817975994568;-0.249445443882614 -0.00204427693199395 0.968386695209115 1.27734054735251;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840344088;-7.9599482888563]
Robot Arm 1 end: [0.997360819914838 -0.0124160865900815 -0.0715348564868221 -9.14378697899978;0.0125488185620241 0.999920271482942 0.00140635350392301 -7.94738998499406;0.0715116917119152 -0.00230031981867633 0.997437109033563 0.731100312735086;0 0 0 1]
Mobile Platform 2 Pose: [-2.35628966743671;9.98351839296815;7.85622741882906]
Robot Arm 2 end: [0.996684567872831 0.0397959204927075 0.0709658852991114 9.39762364970631;-0.0398615460315358 0.999205091717533 -0.000491765931871546 7.88155433778843;-0.0709290442070403 -0.00233867438821725 0.997478621971409 0.734857732874715;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455234098;-7.79314279536571]
Robot Arm 1 end: [0.999793697663594 -0.0123790253860106 -0.0161034729996626 -11.3193241028857;0.0124182214666495 0.999920161559028 0.00233629693573921 -7.8090058656407;0.0160732662444072 -0.00253579144631485 0.999867601172264 0.799171001188807;0 0 0 1]
Mobile Platform 2 Pose: [-2.35628954363625;9.98211897047323;7.68191834696922]
Robot Arm 2 end: [0.999063239247952 0.0397589213075521 0.0170842664416357 10.9161457330495;-0.0397969969439376 0.99920598992416 0.00189439539779647 7.64623350851263;-0.0169953822443962 -0.00257252330190615 0.999852258639359 1.00162520508305;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.9797235609508;-7.63794018922978]
Robot Arm 1 end: [0.999924130629537 -0.012311034909997 0.000414009913926852 -7.79091628714169;0.0123098556276896 0.999920477849512 0.00273960428325934 -7.60341009704214;-0.000447704354938905 -0.00273430002893818 0.999996161574714 2.47325654548136;0 0 0 1]
Mobile Platform 2 Pose: [-2.35591917201006;9.9806688804016;7.51574825035607]
Robot Arm 2 end: [0.999211888391259 0.0396805581075556 0.0010273294730687 8.27552197148218;-0.039683239236753 0.999208563789316 0.0027361604068293 7.58924564757178;-0.000917944035308649 -0.00277477176830405 0.99999572900107 1.83218731218713;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591665116;-7.46636918684545]
Robot Arm 1 end: [0.999911470356552 -0.0121957408037192 0.00532121749661937 -12.0397763896546;0.0121797460910997 0.999921239517226 0.00302796094597304 -7.50088735641034;-0.00535772662182132 -0.00296288180366577 0.999981257872798 -3.02355377029775;0 0 0 1]
Mobile Platform 2 Pose: [-2.35537756396923;9.97930456507438;7.33821082723505]
Robot Arm 2 end: [0.999210216800333 0.0395593381871155 -0.00374184500348848 11.7786314109923;-0.0395476446357107 0.999212723552896 0.0031491099967702 7.26126294516738;0.00386347584439911 -0.00299864172612097 0.999988040779588 -1.62057902911082;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346212;-9.97678818484225;-7.31817266015123]
Robot Arm 1 end: [0.999903707721568 -0.0121148636759247 0.00676796592602333 -9.67949070656626;0.0120932408404523 0.999921667830886 0.00322672097864488 -7.29011847767282;-0.00680652706134838 -0.0031445636283866 0.999971891059419 9.12697151192097;0 0 0 1]
Mobile Platform 2 Pose: [-2.35453920286643;9.97782836159615;7.17878891553962]
Robot Arm 2 end: [0.999207822256069 0.0394618528762938 -0.00514685436499825 8.99530977424378;-0.0394447204086872 0.999216007551473 0.00338884711576145 7.23649305053789;0.00527654945634493 -0.00318314631508717 0.999981012622426 6.68419170767835;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232725;-9.9753590119579;-7.16418511934567]
Robot Arm 1 end: [0.999902164973581 -0.0120040565716842 0.00718074557202058 -3.71065304442149;0.011979613125972 0.999922333528651 0.00343740890350898 -7.11653477562845;-0.00722145071978814 -0.00335105005060925 0.999968310054403 -12.0805986259674;0 0 0 1]
Mobile Platform 2 Pose: [-2.35330614557044;9.97645074292325;7.01004132742525]
Robot Arm 2 end: [0.999211102417486 0.0393245413758297 -0.00554556139744204 6.73068692493919;-0.0393048355504543 0.999220713481647 0.00361879145309174 7.11624974102076;0.00568354713043595 -0.00339796921850137 0.999978075308257 -8.72706352691669;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.78539602676002;-9.97430990779222;-7.00084677780109]
Robot Arm 1 end: [0.999903505384011 -0.0118292599360702 0.0072834421897722 -30.2146663511324;0.0118025797035427 0.999923519060643 0.00369528641959923 -7.2121398652141;-0.00732659764886745 -0.00360896643739427 0.999966647607882 16.5803737834588;0 0 0 1]
Mobile Platform 2 Pose: [-2.35313823381579;9.97523770885185;6.84193839328544]
Robot Arm 2 end: [0.999204704343746 0.039492554135077 -0.00550426972578197 22.4550363526609;-0.0394730425047136 0.999214116305222 0.00360952791858333 6.37700924133433;0.0056424934866795 -0.00338938700386535 0.999978336926851 14.6325802805313;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.78539595660333;-9.97259808478388;-6.91581468798367]
Robot Arm 1 end: [0.999902877167639 -0.01186336777602 0.00731414635403874 34.9462430189383;0.0118362040932421 0.999922932816539 0.0037460245908112 -6.35580498868279;-0.00735802314079842 -0.0036590890372786 0.999966234711391 -7.65591218700211;0 0 0 1]
Mobile Platform 2 Pose: [-2.35298171361147;9.97363439758938;6.72928981092137]
Robot Arm 2 end: [0.999152845944995 0.0399219899254601 0.0099912541959923 -21.1701369238967;-0.0399498335654406 0.999198296664376 0.00260283328365619 7.97194184906897;-0.00987933389004853 -0.00299977722512534 0.999946698628726 -14.5808353472126;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227853;-9.97409553094106;-6.77330520679708]
Robot Arm 1 end: [0.998053762364897 -0.0110326082340252 0.0613756383659581 -81.3808996754909;0.010741268739218 0.999929433539064 0.00507475005475499 -7.7273971175268;-0.0614272950336107 -0.00440562115947987 0.998101837452999 -24.1214617243481;0 0 0 1]
Mobile Platform 2 Pose: [-2.3523426676334;9.97448155201714;6.56760936796531]
Robot Arm 2 end: [0.994331005783598 0.0392086287499287 -0.098835896155842 70.6895516788479;-0.0386151043733322 0.999222837228901 0.00791171754188798 4.13167013233399;0.0990692921727757 -0.00405030761501154 0.995072294035267 4.903080641995;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.97228129699828;-6.73833367577277]
Robot Arm 1 end: [0.999909462618282 -0.0113252276702169 0.00726675888113614 76.3619688737767;0.0112945888428433 0.999927209515049 0.00424357565251757 -5.31119500467831;-0.00731428939063372 -0.00416111639650636 0.999964592513677 100.001227701261;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234262857702;9.97259585499695;6.5135566080018]
Robot Arm 2 end: [0.998670888832204 0.0391365735159661 -0.0335378057201705 -91.2241095502875;-0.0389805175100118 0.999225943295046 0.00529466720172321 10.7345717050117;0.0337190606889662 -0.00398030897729316 0.999423424823883 40.6492046564624;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540074;-9.97333331315079;-6.69737167009219]
Robot Arm 1 end: [0.99991795594276 -0.0105615922026841 0.0072480447982352 -34.8235998712462;0.0105281652380502 0.999933836818013 0.00463462222067109 -7.7793971352972;-0.00729651423443628 -0.00455793336417172 0.999962992376955 -213.071694500175;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234259355258;9.97428696168745;6.47074910251151]
Robot Arm 2 end: [0.992243172225867 0.038343859014135 -0.118250732120579 126.043210732579;-0.0375117980506521 0.999253326804812 0.0092549379535168 1.38160279277175;0.118517307504668 -0.00474735141110215 0.992940637941878 -134.441371005885;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395781195851;-9.97294909644746;-6.68023583759776]
Robot Arm 1 end: [0.999915252679845 -0.0108198915164784 0.0072399865864853 -168.168969937109;0.0107871214463642 0.999931464923829 0.0045501061730217 -7.38861937940048;-0.00728872204863379 -0.00447162194913865 0.999963438895664 325.748462403528;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234257706714;9.9730900183903;6.44689277559749]
Robot Arm 2 end: [0.999241757247752 0.0385408778902982 -0.00552370381810185 -48.9043452365429;-0.0385157573979267 0.999247477505287 0.00458422636717132 9.89958643371458;0.00569622721536345 -0.00436800077455774 0.999974236450492 304.549766785046;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395772638942;-9.97200303864331;-6.67347995257358]
Robot Arm 1 end: [0.999910109869416 -0.0112748239682385 0.00725606817896983 506.203118389378;0.011243589577572 0.999927409075014 0.00433108230598669 -1.47056340630546;-0.00730437364486086 -0.00424910873188142 0.999964294962896 -352.803833198251;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234256665363;9.97332816600719;6.43380367594718]
Robot Arm 2 end: [0.999248689742244 0.0383619730164192 -0.00551498637316858 -169.356023615002;-0.0383362590337496 0.999253865450381 0.00469506397922085 11.6981212649865;0.00569098336897617 -0.00448011258332305 0.999973770305769 -497.2704257219;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395767401621;-9.97358488642586;-6.65848980933812]
Robot Arm 1 end: [0.997141529815597 -0.010452145918313 0.0748299549826968 -1005.55320447591;0.0100629901808542 0.999933821753234 0.00557569211769923 -18.1123330918698;-0.0748832808150803 -0.00480674106579873 0.997180720579122 26.7196212930335;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234256059042;9.97392967764009;6.42661935951389]
Robot Arm 2 end: [0.999265825833431 0.03791447631871 -0.00550470770423432 714.735754791125;-0.0378875784142896 0.999269939786572 0.00491109365026735 -19.1800926088196;0.00568689048005488 -0.0046989280073878 0.999972789305914 526.540778121464;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395764311851;-9.9716500757212;-6.66429501601706]
Robot Arm 1 end: [0.999910258772285 -0.0112663391662711 0.00724872428310446 1511.85265275677;0.0112350289976934 0.999927441774889 0.00434572306377066 15.2296209955371;-0.00729715871849529 -0.00426389344573065 0.999964284705869 813.676896185149;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255707338;9.97235448331124;6.41955572893896]
Robot Arm 2 end: [0.999242566840453 0.0385205095124436 -0.0055193261367116 -1501.02902904588;-0.038495207346342 0.999248107039781 0.00461947927543933 65.9037576576934;0.00569312088961643 -0.00440351272461147 0.999974098389663 -185.090160895934;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785395762440954;-9.97334587567019;-6.65299730614471]
Robot Arm 1 end: [0.995612184959947 -0.0104491558719604 0.0929902806794598 -1351.19763284877;0.00995164501203222 0.999933589041772 0.00581225150824238 -31.2082417716928;-0.0930448382277907 -0.00486134216076672 0.995650051690633 -2373.62078413452;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255495501;9.97427885831256;6.41984840052711]
Robot Arm 2 end: [0.992737151510083 0.0376817959327104 -0.114249858936154 2173.75013993352;-0.0368531069724827 0.999276880907138 0.00935755261681982 -81.6001106324994;0.114519852069937 -0.00507912785696231 0.993407975578056 -1001.71669593832;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785395761295571;-9.9725430019079;-6.65580126947701]
Robot Arm 1 end: [0.99560664286058 -0.0110277221704793 0.0929828050534887 46.0170964404173;0.0105507283990703 0.999928546580899 0.00561995221151401 10.6125495616541;-0.09303813638575 -0.0046142254324393 0.99565185386335 4517.87356091879;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255365341;9.97262184213714;6.41532473535682]
Robot Arm 2 end: [0.998648916641633 0.0385185298169087 -0.0348807131698302 -2415.2839954655;-0.0383380209192366 0.999247824168851 0.00582941213269181 115.091719300853;0.0350790171254586 -0.00448427859978743 0.999374481264632 3302.72075057801;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395760593853;-9.97237811570277;-6.65550571971024]
Robot Arm 1 end: [0.995605819940179 -0.0111418615669588 0.0929780093466597 3450.44579618475;0.010669887497167 0.99992755180443 0.00557177199799001 11.4998902600568;-0.0930333531699391 -0.00455522372913917 0.995652572504453 -6706.85908148388;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255287387;9.97370814485805;6.41620411875585]
Robot Arm 2 end: [0.998679909811465 0.0377194421449901 -0.0348666233414495 300.156899624473;-0.037526290718944 0.999276544073562 0.00617786122175256 -30.8043050471736;0.0350744243550919 -0.00486129084386844 0.999372879663787 -6638.33488138843;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395760172427;-9.97341831965915;-6.64975907402211]
Robot Arm 1 end: [0.995614346699729 -0.0103921795067808 0.0929735190835019 -10301.8682420423;0.00989283989517178 0.999934068897746 0.00583005801047334 -102.703915394432;-0.0930279762462882 -0.00488471725853516 0.995651512916454 5930.5702818107;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255240195;9.97368545240754;6.415311115989]
Robot Arm 2 end: [0.998679320742649 0.0377361138249498 -0.0348654561764442 5595.94316794412;-0.037543151623543 0.999275941311993 0.0061729151299738 -175.448710568592;0.0350731533679647 -0.00485580358134802 0.999372950946946 9232.26822516021;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395759909198;-9.97165700587365;-6.65837171221346]
Robot Arm 1 end: [0.995600584856028 -0.0116237052026048 0.0929750768308265 20469.364700502;0.0111773265457277 0.99992337758679 0.00532036903942049 248.680624240765;-0.0930297952573547 -0.00425774973294467 0.995654221485344 -1882.17744122425;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255211146;9.97262178386111;6.41340973178381]
Robot Arm 2 end: [0.9986491290469 0.038516991448374 -0.0348763304209799 -16030.3450651715;-0.0383364123536543 0.999247871094179 0.00583194663904141 610.564099778849;0.0350747279635648 -0.00448703504732572 0.999374619436959 -9257.58627286524;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395759753156;-9.97359339940187;-6.64819520364775]
Robot Arm 1 end: [0.995616004943816 -0.0102605447019906 0.0929703819618539 -28186.7050205592;0.00975772890984451 0.999935213745836 0.00586131705270495 -385.776150122854;-0.0930244990646901 -0.0049284412838947 0.99565162232594 -15181.8067871836;0 0 0 1]
Mobile Platform 2 Pose: [-2.3523425519394;9.97433287083715;6.41584052632897]
Robot Arm 2 end: [0.998698387086311 0.0372333400884951 -0.0348598625507276 28719.9551285569;-0.0370351590800753 0.999294016867141 0.00631386137384416 -1112.16522167535;0.035070338223557 -0.00501460261526897 0.999372265543474 -3373.23689186758;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785395759655636;-9.97198118931604;-6.65650177558254]
Robot Arm 1 end: [0.995603320522648 -0.0114107853958458 0.0929721578803394 31681.8042510381;0.0109521698021356 0.99992521679537 0.00544158004231344 548.612475104918;-0.0930272978265091 -0.0043994082990406 0.995653838975031 44427.6510685911;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255183207;9.97241737622826;6.41258494875635]
Robot Arm 2 end: [0.998643565469538 0.0386590694962515 -0.0348784387834654 -41241.4327308331;-0.0384813869648275 0.99924276441271 0.00575157596744742 1757.15055330965;0.0350743781634281 -0.00440160363164502 0.999375011635731 28475.5569598576;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395759597111;-9.97295374746973;-6.65125334903681]
Robot Arm 1 end: [0.995611092089375 -0.0107327911765035 0.0929696751750959 -7502.89198085192;0.0102441067676928 0.999931098657233 0.00573203407792233 -425.74635124906;-0.0930247901644147 -0.00475448542956236 0.995652441006984 -88010.3489874196;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255176836;9.97397279351317;6.41510623975141]
Robot Arm 2 end: [0.998687812129461 0.037514411147467 -0.0348614810403854 27868.6402721077;-0.0373188860179284 0.999283908150965 0.00624272832290651 -1369.15998860434;0.0350707092949546 -0.0049335450531598 0.999372655961107 -75058.9607250847;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785395759561427;-9.97296922404441;-6.6513492560343]
Robot Arm 1 end: [0.995611239736982 -0.0107215343706841 0.0929693928673796 -62264.5275811783;0.010232448990432 0.999931196326015 0.00573581743810963 -262.757453376312;-0.0930244929953888 -0.00475933974027443 0.995652445579374 128556.418974336;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255172935;9.97336155712662;6.41384590669757]
Robot Arm 2 end: [0.998670052695809 0.0379796509970687 -0.0348664876162111 27169.0251174779;-0.0377899323084579 0.999267181431672 0.00608449913498347 -560.364890855546;0.0350720239603122 -0.00475880486491124 0.999373457177839 129588.439889607;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785395759539366;-9.97196650325642;-6.6562907865685]
Robot Arm 1 end: [0.995603284974502 -0.0114204647952065 0.0929713500592645 191892.30519246;0.0109623646464971 0.999925132432755 0.00543655136047116 1876.69467306697;-0.0930264774638817 -0.0043934625523909 0.995653941878132 -150202.203032048;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255170554;9.972897465247;6.41327994258668]
Robot Arm 2 end: [0.998656862881016 0.0383202626870586 -0.0348715885534721 -149791.343289696;-0.0381360042764924 0.999254938624765 0.00593403840910313 5310.6865722178;0.0350730009903798 -0.00459620513164709 0.99937418392708 -166868.862793447;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.78539575952626;-9.97359580921727;-6.64798611087732]
Robot Arm 1 end: [0.995616183271725 -0.0102584027979264 0.0929687086039077 -378054.201484501;0.0097555106495841 0.999935230660109 0.00586212390520802 -4200.35521888529;-0.0930228231102919 -0.00492946820150565 0.995651773826493 29548.7155349618;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255169139;9.97424212078951;6.41527493497345]
Robot Arm 2 end: [0.998695747560058 0.0373042990117996 -0.0348596196292418 344521.283422038;-0.0371067231762728 0.999291463034669 0.00629785686955626 -12996.0085907478;0.0350698574359324 -0.0049961166187553 0.999372375002509 72080.049244356;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785395759518025;-9.97165958807069;-6.65794808034011]
Robot Arm 1 end: [0.995600876771844 -0.011621694662932 0.0929722022127923 615946.857563428;0.0111751672413712 0.999923393042451 0.00532199996240445 8646.93460776362;-0.0930269305538035 -0.00425960792022305 0.995654481199228 227091.874769013;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255168256;9.97233254983105;6.41224224095459]
Robot Arm 2 end: [0.998641285259475 0.0387171694609357 -0.0348792798698038 -589633.912490355;-0.0385407356242232 0.999240676175185 0.00571688503487252 24186.4767732565;0.0350741368082902 -0.00436484431468036 0.999375181331447 165844.325452338;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785395759513141;-9.97340627578782;-6.64887127655015]
Robot Arm 1 end: [0.99561471550551 -0.0104002285016911 0.0929686695397714 -622984.004364802;0.00990104528157638 0.999933993772886 0.00582901361615117 -10337.6694508458;-0.0930231561022026 -0.0048829647262409 0.995651971867815 -825231.60165635;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255167734;9.9741785679805;6.41525178642638]
Robot Arm 2 end: [0.998693876251414 0.0373540711956016 -0.0348599326310209 598882.000257724;-0.0371569489836394 0.999289673077547 0.00628573168343585 -26114.1550902102;0.0350699683511777 -0.00498223300166293 0.99937244042157 -758020.837109011;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785395759510112;-9.97238834370791;-6.65431341078414]
Robot Arm 1 end: [0.995606657991141 -0.0111335992488869 0.0929700249084432 270262.908032517;0.0106611590525685 0.999927617043229 0.0055767698451692 9520.57005124865;-0.0930253849837087 -0.00456110096526451 0.995653290109874 1705517.45229766;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255167412;9.97303062706072;6.41320582041272]
Robot Arm 2 end: [0.998660620977986 0.0382237351044442 -0.0348699036782146 -215546.213690165;-0.0380378172297468 0.999258407654973 0.00597989893263277 14624.4384584299;0.0350726184973274 -0.00464551455851784 0.99937396935593 1611759.38737705;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785395759508273;-9.97251220481971;-6.6534707095392]
Robot Arm 1 end: [0.995607645434637 -0.0110472937155733 0.0929697459051017 961279.375483982;0.0105709982293338 0.99992836572071 0.00561403807838674 1820.7010156125;-0.0930251060119405 -0.00460659621325858 0.995653106720807 -2698709.16569358;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255167217;9.97322040677572;6.41371101213733]
Robot Arm 2 end: [0.998666016546966 0.0380844494547996 -0.0348677803127763 -1099049.7790962;-0.0378962945761634 0.999263413986439 0.00604154993061884 33785.7965502229;0.0350721862964353 -0.0047121309290308 0.999373672642269 -2476200.93079621;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785395759507166;-9.97332888203844;-6.64945225394588]
Robot Arm 1 end: [0.995614113514804 -0.0104577239149339 0.0929686666606069 -3570605.95031385;0.00996017155882421 0.999933492656173 0.00581422797503419 -30789.6257131403;-0.0930232871524696 -0.00486274356160186 0.995652058588945 2793563.59556249;0 0 0 1]
Mobile Platform 2 Pose: [-2.352342551671;9.97406848563032;6.41488019180202]
Robot Arm 2 end: [0.998690637318565 0.0374399676049537 -0.034860575984606 3699380.12495159;-0.037243673975371 0.99928658424447 0.00626348927011623 -134553.652385105;0.0350702107358188 -0.00495695216444606 0.999372557630129 2188716.33589062;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785395759506474;-9.97169930686996;-6.65771324223667]
Robot Arm 1 end: [0.995601197195301 -0.0115960136594769 0.0929719775550217 7628563.17739578;0.01114793364489 0.99992361476848 0.00533743419503892 91153.6043330109;-0.0930267688288234 -0.00427751043791431 0.995654419558173 -1782474.23032732;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255167028;9.9723668273255;6.41226585962543]
Robot Arm 2 end: [0.998642214810537 0.0386936822442353 -0.0348787292298172 -7457281.37441513;-0.0385167371827165 0.999241519972579 0.00573111853718164 296698.382464546;0.0350740324898964 -0.0043799220623008 0.999375119025697 -382141.154414288;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785395759506065;-9.97361778204734;-6.64785910460435]
Robot Arm 1 end: [0.995616367947695 -0.0102418396240609 0.0929685570273941 -11399858.0095542;0.00973854076523503 0.999935375011147 0.00586571607575685 -146024.253706467;-0.0930226246587606 -0.00493462485226109 0.995651766823704 -4269416.87606177;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166985;9.97430441203271;6.41537088120025]
Robot Arm 2 end: [0.998697589154717 0.0372553265462965 -0.0348592320696373 9725548.95460933;-0.0370573215227462 0.999293223996706 0.00630931024990688 -400198.987556295;0.0350696498145593 -0.00500930316497058 0.999372316278416 -6470196.44498321;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785395759505809;-9.97187540409097;-6.656938911363]
Robot Arm 1 end: [0.995602587129687 -0.0114808509184076 0.0929713857208971 14432100.5488076;0.0110260833398537 0.99992460998412 0.00540368710128496 235168.310717649;-0.0930264155326708 -0.00435481461089832 0.995654117453871 15064225.0663815;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166958;9.97272922485312;6.41269581750572]
Robot Arm 2 end: [0.998652166823151 0.0384405990440933 -0.0348736296441238 -9074689.96285381;-0.0382585366445628 0.999250613843029 0.00587325362512063 426474.391106475;0.0350732672165127 -0.00453112342136002 0.999374471780873 17780123.1033043;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785395759505656;-9.97307917302321;-6.65059553927168]
Robot Arm 1 end: [0.995612148533451 -0.0106414737852343 0.0929688589167511 -7289790.19451002;0.0101496424157745 0.999931892516835 0.00576151772822992 -209245.292683304;-0.0930238380816256 -0.00479263637040895 0.995652347049503 -32717160.8703355;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166943;9.97354981292536;6.41418747655178]
Robot Arm 2 end: [0.998675493398609 0.037838117396008 -0.0348645343719054 -3429267.6503434;-0.037646436778366 0.999272268980733 0.00613826066486971 7954.76930668812;0.0350714225964153 -0.00481760500905893 0.999373196558243 -32492343.1379325;0 0 0 1]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785395759505562;-9.97283566172529;-6.65207568829744]
Robot Arm 1 end: [0.995610219719845 -0.0108178291612196 0.0929691613474084 -16454813.889171;0.0103322415177844 0.99993035863496 0.00570286466421955 -2516.39569169929;-0.0930243794637727 -0.00471725051262888 0.995652656488688 52085162.1598021;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166933;9.97382056263726;6.41440045431024]
Robot Arm 2 end: [0.998683370406672 0.0376317193808432 -0.03486229151407 34058930.7992404;-0.0374374859214168 0.999279689491193 0.0062078030145695 -1167674.00906889;0.0350707901401472 -0.00489447308966464 0.999372845244516 40117534.4321551;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036872e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782875e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785395759505504;-9.97208304294275;-6.65574885789258]
Robot Arm 1 end: [0.995604233017218 -0.0113423293730619 0.0929707629450784 64570625.6775219;0.0108800799524192 0.999925808449076 0.00547735864188958 570222.461444525;-0.0930259913057956 -0.00444175211553885 0.995653773045489 -66454000.4325242;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166927;9.9725149379314;6.412478590375]
Robot Arm 2 end: [0.998646251743669 0.0385913091736577 -0.0348765642600418 -84571321.1968358;-0.0384122100766361 0.99924519825748 0.0057910168707676 3250884.5100369;0.0350737222910547 -0.00444349137866839 0.999374849287803 -33016818.2693379;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.811171e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.140248e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.78539575950547;-9.97355589919412;-6.64829011432792]
Robot Arm 1 end: [0.995615887833542 -0.010288357126316 0.092968562434131 -141162822.240169;0.00978620967352745 0.999934969192143 0.00585555180432138 -1493574.75355871;-0.0930227606215452 -0.0049200705633879 0.995651826148076 31103006.8913202;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166923;9.97434385975798;6.41535115823924]
Robot Arm 2 end: [0.998698754241733 0.0372242662594654 -0.034859034373529 136523634.257212;-0.0370259989579053 0.999294340996783 0.00631628474461271 -5403491.71964048;0.0350695548471839 -0.0050173751354929 0.999372279118032 -37669006.2343541;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563931e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.131772e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785395759505448;-9.97163424244322;-6.65815397495836]
Robot Arm 1 end: [0.995600686360197 -0.01163800516955 0.0929722009784546 244914598.485787;0.0111924742568851 0.99992325215187 0.00531209243489401 3315570.56939086;-0.093026887721312 -0.00424813390813198 0.995654534223184 52795452.3080213;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166921;9.97249494775453;6.41230851499676]
Robot Arm 2 end: [0.998645704366942 0.0386052135375763 -0.0348768495878333 -176950079.813134;-0.0384264001445774 0.999244698652391 0.00578307764269841 7674310.5510982;0.03507376400364 -0.00443505386785694 0.999374885303711 169036920.195444;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056079e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.164804e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785395759505435;-9.97348102378628;-6.64859180451582]
Robot Arm 1 end: [0.995615304755753 -0.0103444460294976 0.0929685827172419 -276643625.838955;0.00984374981717842 0.999934480032363 0.00584262200932008 -4239890.24996092;-0.0930229301067571 -0.0049018444232557 0.995651900211918 -278783330.992382;0 0 0 1]
Mobile Platform 2 Pose: [-2.3523425516692;9.97384898707962;6.41461352014959]
Robot Arm 2 end: [0.998684201237867 0.0376098622013726 -0.0348620777217479 80952206.4265819;-0.0374153790003547 0.999280475361797 0.00621457761717995 -4658122.46836533;0.03507072300571 -0.00490202263294507 0.999372810597708 -385351444.642448;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231848e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.658996e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785395759505427;-9.97225080614539;-6.65511445526811]
Robot Arm 1 end: [0.99560556658554 -0.011228349677524 0.0929703175610657 197163137.396726;0.0107603795414275 0.999926795655678 0.00553331382558532 4714814.6146564;-0.0930256417124362 -0.00450860214337336 0.995653505236989 630522286.728663;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166919;9.97351681834694;6.41384906699257]
Robot Arm 2 end: [0.998674538520957 0.0378630365618431 -0.0348648328874393 231820292.045432;-0.037671688433634 0.999271373136616 0.00612916961914102 -6735362.59488479;0.0350714984069934 -0.004807628519086 0.999373241940923 602482050.286643;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531990e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402583e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785395759505423;-9.97265434030515;-6.65284295629283]
Robot Arm 1 end: [0.995608778682583 -0.0109471659043481 0.0929694539557908 224224063.820261;0.0104666479898941 0.99992923491404 0.00565459495269161 -1222452.73379257;-0.0930247767534589 -0.00465668582642606 0.995652904473785 -1064188219.93896;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166919;9.97275767441877;6.41283559887305]
Robot Arm 2 end: [0.998652958455736 0.0384203667494116 -0.0348732560344801 -849298698.360769;-0.0382379246810628 0.999251340909945 0.00588377478855349 31105303.066651;0.0350732046395972 -0.00454236816184635 0.999374422930561 -700402833.911634;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692260e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.092552e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.78539575950542;-9.97322813797382;-6.65009145689996]
Robot Arm 1 end: [0.995613323291367 -0.0105321914270181 0.0929687228505616 -1198407716.47578;0.0100368633878253 0.99993284393455 0.00579387558515719 -9249534.68807255;-0.0930235016436923 -0.00483534535548338 0.995652171984393 1250085169.8901;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97429542286875;6.41518420661955]
Robot Arm 2 end: [0.998697323879497 0.0372623960191491 -0.0348592759368853 1719822785.22635;-0.0370644516813757 0.999292969758149 0.00630769478479629 -65902684.7389533;0.0350696691956278 -0.00500743795281429 0.999372324945842 89896794.1402147;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663859e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.038150e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.785395759505418;-9.97176377613675;-6.65747813759201]
Robot Arm 1 end: [0.995601705826407 -0.0115541049946298 0.0929717484686675 2784765303.62496;0.0111035404808792 0.999923976806481 0.00536208880014745 31688942.4273821;-0.0930266345964293 -0.00430618918351951 0.995654308477786 -1070472469.70915;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97235787119468;6.41208009338539]
Robot Arm 2 end: [0.998641972283656 0.0386998229479798 -0.0348788603197523 -2721097987.51685;-0.0385230108289031 0.999241299339645 0.00572741920010427 113290428.525379;0.0350740478143881 -0.00437600249228979 0.999375135658328 1248704056.19589;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984807e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.086466e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785395759505417;-9.97362810088841;-6.64792805207277]
Robot Arm 1 end: [0.995616448404972 -0.0102340679114633 0.0929685512388633 -4519296462.68536;0.00973058118175347 0.999935442817009 0.00586736637826334 -55272506.3077974;-0.0930225964770652 -0.00493700843983752 0.995651757640356 -1018761573.15517;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97408995051509;6.41493999827519]
Robot Arm 2 end: [0.998691268786795 0.0374232493594104 -0.0348604368361226 2358470022.99144;-0.0372267898465803 0.999287185396967 0.00626795174887208 -107492784.841723;0.0350701549289484 -0.00496200652871715 0.999372534505761 -4120975164.38602;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059253e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.365674e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785395759505416;-9.97178688319742;-6.65748565585436]
Robot Arm 1 end: [0.995601888099908 -0.0115390127402201 0.0929716709373279 6283243268.46621;0.0110875678951169 0.999924107221773 0.0053708132449029 96846285.2115949;-0.0930265889413824 -0.00431636209341724 0.995654268693812 4907958971.56652;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97318626353133;6.41327379622287]
Robot Arm 2 end: [0.998665042571332 0.03810965247312 -0.0348681392445426 223768295.340731;-0.0379218888893698 0.999262508061521 0.00603078151318269 21455933.7190598;0.0350722552605488 -0.00470046497439037 0.999373725159894 7924061913.20647;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857436e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.453922e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785395759505416;-9.97319545975326;-6.65010797578431]
Robot Arm 1 end: [0.995613066042222 -0.0105562511207103 0.0929687489874061 -4405406132.81335;0.0100616690052558 0.999932634410383 0.00578700768445894 -96706442.9876357;-0.0930235751991692 -0.00482619968379007 0.99565220948571 -11934340172.987;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97306387551347;6.41328693886878]
Robot Arm 2 end: [0.998661563157833 0.0381994747283385 -0.0348695053253262 -7144633258.0449;-0.0380131539771999 0.999259279582026 0.00599101776961113 239381353.874028;0.0350725305026544 -0.00465749929566579 0.999373916662053 -11234839846.3699;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905819e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.897855e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785395759505415;-9.97269674450398;-6.65290069318509]
Robot Arm 1 end: [0.995609115971728 -0.0109170771676425 0.0929693800135869 -3339059194.12204;0.0104353417287641 0.999929496240942 0.00566623246438354 36141289.8157471;-0.0930246840198821 -0.00467118544400209 0.995652845217423 20624963277.1304;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97416087764874;6.41487749542401]
Robot Arm 2 end: [0.998693356396573 0.0373678939918539 -0.0348600113017974 19520736861.7627;-0.0371709009880062 0.999289175944163 0.00628227351953424 -723538166.944417;0.0350699872980832 -0.00497828679848507 0.999372459422144 7752165640.40593;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634522e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.328409e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785395759505415;-9.97221078172521;-6.65519569974022]
Robot Arm 1 end: [0.99560524822679 -0.0112557048928464 0.0929704190086187 20909570354.3091;0.0107890730051842 0.999926558639633 0.00552025640603164 157576427.748117;-0.093025725511612 -0.0044929316113961 0.995653568244786 -28334793194.3588;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97233572440888;6.41203255560753]
Robot Arm 2 end: [0.998641372907993 0.0387149861390046 -0.0348791938653236 -36350202806.404;-0.0385385042172986 0.999240754550904 0.00571822850474468 1467215755.85479;0.035074093133414 -0.0043662676047051 0.999375176646925 3743035913.66252;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.067005e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.828036e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735006134901;-6.64870124197991]
Robot Arm 1 end: [0.995615457499034 -0.010329792561095 0.0929685762751767 -51661908679.1099;0.00982871053200334 0.999934607810815 0.00584607145957359 -518823233.047552;-0.0930228855619232 -0.00490667788601422 0.99565188056562 19244671359.4689;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97425523644131;6.41513676395269]
Robot Arm 2 end: [0.998696137931533 0.0372939718491008 -0.0348594857156573 42473721626.5685;-0.0370963033180007 0.999291834236421 0.00630034193897467 -1790018338.57468;0.0350697641962493 -0.00499896910648453 0.999372364010078 -38536970353.3653;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.069100e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.488255e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163094321734;-6.65826420090662]
Robot Arm 1 end: [0.995600660473449 -0.0116401262841468 0.0929722126493843 95318370181.4156;0.011194725748195 0.999923233839107 0.00531079505959105 1243819900.49699;-0.0930268938547125 -0.00424663264615525 0.995654540054387 6906289269.41555;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97286607273715;6.41273283526264]
Robot Arm 2 end: [0.998655987236052 0.0383428162224304 -0.0348718740780948 -29817187730.1702;-0.0381589574888136 0.999254127869622 0.00592299745799969 1528733703.17702;0.035072968522104 -0.00458436251331402 0.999374239461571 92669612659.7582;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.776572e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.703599e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785395759505415;-9.97354092061521;-6.64840078543374]
Robot Arm 1 end: [0.995615771368264 -0.0102995948637712 0.0929685653778717 -118441134054.26;0.00979773264443023 0.99993487117216 0.00585302049756656 -1701295043.17097;-0.0930227941840369 -0.00491647836961037 0.995651840756916 -90800600255.3807;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97339519806848;6.41377258543003]
Robot Arm 2 end: [0.998671029173561 0.0379544284847461 -0.0348659841079745 -40278394447.1988;-0.037764345802863 0.999268087720114 0.00609450984640476 979735855.812396;0.0350717789041706 -0.00476971934000677 0.99937341374579 -155557206341.032;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247228e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.540971e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.785395759505415;-9.97212038216775;-6.6558824117393]
Robot Arm 1 end: [0.995604529646221 -0.0113171166212849 0.0929706589161703 109616959666.912;0.0108535686738558 0.999926026741154 0.00549009040831598 2180491948.448;-0.0930259135669655 -0.00445689544748638 0.99565371263708 227692267060.857;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97394654861737;6.41444848978556]
Robot Arm 2 end: [0.998687057727091 0.0375345856705229 -0.0348613770087775 195041308119.576;-0.0373392718943205 0.999283181993386 0.00623706337755753 -6910811901.5244;0.0350704933356798 -0.00492717603864917 0.999372694960932 167138230251.092;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064539e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.494692e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785395759505415;-9.97279446002223;-6.65223019561338]
Robot Arm 1 end: [0.995609892634541 -0.0108473716283264 0.0929692218801194 30214712111.2733;0.0103629046322788 0.999930101867374 0.00569223919786097 -1170569927.31384;-0.0930244693390948 -0.00470381847655164 0.995652711639917 -411665441464.086;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97243137984914;6.41216660936843]
Robot Arm 2 end: [0.99864396894582 0.0386492485692643 -0.0348777704730506 -434103079601.509;-0.038471354126824 0.999243116443156 0.0057575300795504 16978584184.4259;0.0350738962732697 -0.00440792763094434 0.999375000675028 -97252840093.9426;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.742991e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.506119e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.785395759505415;-9.97311524782296;-6.65079308472694]
Robot Arm 1 end: [0.995612433583605 -0.0106151025624425 0.0929688210709421 -384967498587.791;0.010122401770401 0.999932122041652 0.00576960065142295 -2493716212.91044;-0.0930237554398372 -0.00480321838636816 0.995652303777279 538258055866.21;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97433609960919;6.41518861982485]
Robot Arm 2 end: [0.998698525022961 0.0372303801742347 -0.0348590721196762 633940592034.135;-0.037032163916053 0.999294121123301 0.00631492866969635 -25523970065.5382;0.0350695730321515 -0.00501580307535359 0.999372286371226 -283420406717.332;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155980e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.140273e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785395759505415;-9.97184705328127;-6.6571492964578]
Robot Arm 1 end: [0.995602363265157 -0.011499537502284 0.0929714735715611 994500322434.368;0.011045825471367 0.99992444842268 0.00539325377255467 10672049662.7146;-0.0930264693541038 -0.00434258953075688 0.995654165820481 -548177423360.959;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97259560445058;6.41228431780473]
Robot Arm 2 end: [0.998648468435693 0.0385349286578857 -0.0348754321030908 -721913739311.803;-0.0383546926469104 0.99924722414926 0.00582259237456784 32240920970.6996;0.0350735519017774 -0.00447708647794528 0.999374705330119 951399207968.082;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302894e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.521201e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362257766344;-6.64808091524349]
Robot Arm 1 end: [0.995616405589813 -0.0102382277492186 0.0929685517418498 -1757655023760.11;0.00973484137926948 0.999935406520532 0.00586648531391443 -20557176936.3223;-0.0930226089923431 -0.00493573491721759 0.995651762785104 -148856025782.999;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97371282971994;6.41423110149212]
Robot Arm 2 end: [0.998680227836105 0.0377142431792074 -0.0348631379919339 114023422199.111;-0.0375209878572084 0.999276722449752 0.00618121673294429 -11995678714.9558;0.0350710421779028 -0.00486495955790162 0.999372980507806 -1933538969491.02;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531498e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446534e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.785395759505415;-9.97171662409789;-6.6579385588234]
Robot Arm 1 end: [0.995601334261794 -0.0115847848783861 0.092971909604226 2642776019577.2;0.0111360333301568 0.999923711752878 0.00534410291970526 38911533827.7773;-0.0930267272229037 -0.00428525771316992 0.9956543901317 1514108597637.71;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97366632068432;6.41392684692148]
Robot Arm 2 end: [0.998678874425951 0.0377497098898915 -0.0348635221763959 1584581175226.9;-0.0375568922492127 0.99927544733264 0.00616929515698474 -51220147015.6567;0.0350711508208107 -0.00485177919757206 0.999373040770423 2720361398143.97;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.613167e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.280300e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785395759505415;-9.97330001676718;-6.64968226679352]
Robot Arm 1 end: [0.995613888239926 -0.0104791036962217 0.0929686717636597 -2264489920426.13;0.00998217687373422 0.999933306365686 0.00580852523877533 -42507618249.7042;-0.0930233394833616 -0.00485501867265587 0.99565209139792 -4265041826178.22;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97263245096147;6.41246096168395]
Robot Arm 2 end: [0.99864948504487 0.0385090322759513 -0.0348749258463052 -4634274017917.7;-0.0383282855219472 0.99924815470001 0.00583676773333374 174135061862.678;0.0350734735742501 -0.00449218897582633 0.999374640307449 -2774066720581.01;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408568e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.794404e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785395759505415;-9.97255494590405;-6.6537371298561]
Robot Arm 1 end: [0.995607987690886 -0.0110173024802019 0.0929696396258683 -117438706260.762;0.0105397142562728 0.999928625988961 0.00562648657898821 27954722571.6671;-0.093024992714325 -0.00462190154451092 0.995653046375399 7992132659880.02;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97432934502904;6.41509143550358]
Robot Arm 2 end: [0.998698325518969 0.0372357003239653 -0.0348591053670396 8389300165166.34;-0.0370375287138707 0.99929392979681 0.0063137419979903 -326338139232.763;0.0350695889963901 -0.00501442844518014 0.999372292709275 -848604490875.776;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394164e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.099115e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785395759505415;-9.97234650754405;-6.65460270477929]
Robot Arm 1 end: [0.995606328211195 -0.0111625388247224 0.0929700863332282 6422830923487.38;0.0106914368429707 0.999927366081828 0.00556378817811232 36499256220.2409;-0.0930254395531358 -0.00454535891263802 0.995653357001472 -11734872273325.1;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97240935962833;6.41197672227929]
Robot Arm 2 end: [0.998643369671087 0.0386644379254893 -0.0348780941549714 -12174946040141.9;-0.0384868654965322 0.999242570691977 0.00574857383550303 515677045951.946;0.0350739418404733 -0.00439842662737202 0.999375040936572 8105149338931.38;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956357e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.353663e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785395759505415;-9.97343026926908;-6.64918949930142]
Robot Arm 1 end: [0.9956149085221 -0.0103823401063568 0.0929686019172367 -18489670798798.9;0.00988266363739851 0.999934149649173 0.00583346593506901 -174729008978.264;-0.0930230449295214 -0.00488910823172327 0.995651952106126 10033360037270.8;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97398422522029;6.41460970814652]
Robot Arm 2 end: [0.998688162760446 0.0375054115959857 -0.0348611196489792 8470167019724.88;-0.0373097881204539 0.999284231003291 0.00624542861362848 -413988186239.592;0.0350704045110914 -0.00493657464005094 0.999372651696179 -21755255513331.9;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260469e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.096609e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164968951921;-6.65826235962286]
Robot Arm 1 end: [0.995600807731558 -0.011628063354191 0.0929721452204895 36354133746698;0.0111819234226831 0.999923337989306 0.00531815126654809 456566941242.037;-0.0930268575887531 -0.0042551482883158 0.995654507085769 -2431097058758.44;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97334425293488;6.41335609887114]
Robot Arm 2 end: [0.998669564328545 0.0379924891418592 -0.0348664889634746 7272806517754.93;-0.0378029550791464 0.99926671954612 0.00607945678661814 -139452555064.381;0.0350718957445763 -0.00475331214439237 0.999373487817512 37954243631341.7;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807923e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.758887e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785395759505415;-9.97358545598195;-6.64829065134618]
Robot Arm 1 end: [0.995616117547836 -0.0102661566985368 0.0929685565425967 -49277296667592.4;0.00976345383863963 0.999935162851721 0.00586046600905479 -669320930510.06;-0.0930226931888862 -0.00492708020470759 0.99565179778502 -28117883532900.5;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97291301128612;6.41287457346687]
Robot Arm 2 end: [0.998657304818422 0.0383090117886425 -0.0348712940480048 -42903346782572.1;-0.0381245543700323 0.999255342759272 0.00593955563461422 1512585657176.38;0.0350728653932241 -0.00460212807599637 0.999374161428182 -49361284230227.1;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620323e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.152133e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.785395759505415;-9.97200013539397;-6.65659579684792]
Robot Arm 1 end: [0.995603575202781 -0.0113979848180335 0.0929710007772743 54075004909353.7;0.0109386699196357 0.999925326750308 0.0054485249086245 962357299743.789;-0.0930261605347082 -0.00440759178900716 0.995653909042089 80098536844721.1;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97423520987965;6.41484952703523]
Robot Arm 2 end: [0.998695547206598 0.0373096869183689 -0.0348595933964585 100156225255494;-0.0371121584883438 0.999291269102489 0.00629660129544798 -3773733342767.44;0.0350698115485266 -0.00499467292133061 0.999372383829151 22696220046121.9;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640783e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.565949e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785395759505415;-9.97292968713781;-6.65164343752433]
Robot Arm 1 end: [0.995610965692258 -0.0107500832907721 0.0929690308789489 -9557651571534.47;0.0102620075944947 0.999930947644771 0.00572635506762414 -699535151799.742;-0.0930241699423329 -0.00474717299784144 0.995652533846556 -156352258263789;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97233137427166;6.41184191396592]
Robot Arm 2 end: [0.998641255297085 0.038717960482281 -0.0348792596646739 -172833093319397;-0.0385415436420385 0.999240647688395 0.00571641679417336 7076477907861.89;0.0350741020177575 -0.0043643491345666 0.99937518471507 45511644767373.8;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711110e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.989234e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785395759505415;-9.97299183541534;-6.65154595274172]
Robot Arm 1 end: [0.995611457985964 -0.010705061378499 0.092968953892932 -116068617086611;0.0102153943622486 0.999931339237106 0.00574130033271942 -530692851999.144;-0.0930240315460865 -0.00476638986753207 0.995652454967366 224772222413010;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9741869719729;6.41487074892698]
Robot Arm 2 end: [0.99869412515015 0.0373474826327488 -0.0348598613246429 178165518372468;-0.0371502989036374 0.999289909940243 0.0062873828412126 -7741118684467.73;0.0350699256050994 -0.00498411803813904 0.999372432522248 -218237451489608;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373557e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.265278e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785395759505415;-9.97194709741018;-6.65673575840169]
Robot Arm 1 end: [0.995603154857896 -0.0114333494170032 0.0929711598191224 346519725077500;0.0109759487438942 0.999925020791521 0.00542967257282943 3453492454036.58;-0.0930262682588928 -0.00438535245851929 0.995653997178558 -257982507953210;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97301420853955;6.41279137581879]
Robot Arm 2 end: [0.998660157385614 0.0382356832036762 -0.0348700814522725 -71049802948045;-0.03804996559776 0.999257978196586 0.00597437264319557 4582703193925.03;0.035072641311274 -0.00463956252448394 0.999373996205141 469276169563100;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767804e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.485788e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360123983821;-6.64831637142007]
Robot Arm 1 end: [0.995616240078673 -0.0102542878063165 0.0929685542169566 -671023667973013;0.00975129241146806 0.999935266397928 0.00586304613756389 -7504323596951.81;-0.093022657390079 -0.00493078039364818 0.995651782812042 35884310965133.5;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97323738886507;6.41335175390775]
Robot Arm 2 end: [0.998666502217863 0.0380718850340624 -0.034867591225823 -308944324218654;-0.0378835371238577 0.999263865592963 0.00604686159875366 9370165122891.18;0.0350721394118357 -0.00471789043559988 0.999373647114489 -727125819863499;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002526e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.098915e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166632921239;-6.65828886791101]
Robot Arm 1 end: [0.995600938518992 -0.0116173346368092 0.092972085896999 1.08076277256754e+15;0.011170541313102 0.999923430630439 0.00532464862925608 15286772154410.1;-0.0930268253079446 -0.00426267664609352 0.995654477899304 429582672638706;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97405755545236;6.41447567701459]
Robot Arm 2 end: [0.998690316303768 0.0374484700363779 -0.0348606398824609 1.06861927199002e+15;-0.0372522615096458 0.999286278496182 0.00626119969871212 -38782701670537.1;0.035070231443449 -0.00495436183425284 0.999372569748299 656789897160627;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.187228e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.309829e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785395759505415;-9.97339148554344;-6.64932526492237]
Robot Arm 1 end: [0.995614605218901 -0.0104112262161142 0.0929686196707673 -1.06933442838942e+15;0.00991234854528724 0.999933897855798 0.00582625653631706 -18020404320814.6;-0.0930231327204564 -0.00487916873935846 0.995651992661736 -1.48932325070855e+15;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9723717520821;6.41189112138406]
Robot Arm 2 end: [0.998642348472475 0.0386903016094025 -0.0348786524790487 -2.17350905816564e+15;-0.0385132835023713 0.999241641429255 0.00573315163188085 86336743034473;0.035074019319815 -0.004382076578704 0.999375110043077 -147278703985297;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546799e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.844113e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785395759505415;-9.97241328591609;-6.65456938368534]
Robot Arm 1 end: [0.995606859848169 -0.0111162877294815 0.0929699347659425 400172850492457;0.0106430575322245 0.999927767150004 0.00558442555438023 15959788004132.5;-0.0930252973638568 -0.00457040802577128 0.995653255617057 3.03164564220283e+15;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97430856285068;6.41499247515538]
Robot Arm 2 end: [0.998697711813486 0.0372520597562702 -0.0348592091479731 2.86665232186868e+15;-0.0370540267804412 0.999293341472296 0.00631005455496539 -117692592957770;0.0350696381199065 -0.00501016297614821 0.999372312378671 -1.84116199731263e+15;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.136121e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.367392e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724870348897;-6.65398600376929]
Robot Arm 1 end: [0.995607447036979 -0.0110649010554381 0.0929697766435018 1.81854245989743e+15;0.0105893753809131 0.999928212921616 0.00560661505954108 4843100162471.65;-0.0930251392557482 -0.00459749584199108 0.995653145677967 -4.7434149825572e+15;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97271539493323;6.41229216129895]
Robot Arm 2 end: [0.998651782591782 0.0384504152223155 -0.0348738110261596 -2.7374783327241e+15;-0.0382685385325051 0.999250261089798 0.00586810621551707 127837620251288;0.0350732958936415 -0.00452562495153415 0.999374495689156 5.14296239938114e+15;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235454e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.601924e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785395759505415;-9.97334546906772;-6.64975107323676]
Robot Arm 1 end: [0.995614244801164 -0.0104454184661102 0.0929686440800137 -6.44798221205574e+15;0.00994751050064725 0.999933599869602 0.00581746393701195 -56478323064474.9;-0.0930232367951575 -0.00486714340108929 0.995652041795358 4.78745201519743e+15;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97356608998757;6.41383045949808]
Robot Arm 2 end: [0.9986759649473 0.0378258043674934 -0.0348643881422261 -849155304593489;-0.0376339614108934 0.999272711636707 0.00614269702533536 -3670559051953.79;0.035071384134406 -0.00482247884119675 0.999373174401196 -9.48938056146969e+15;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.018100e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.083654e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785395759505415;-9.97169001677896;-6.65815098765241]
Robot Arm 1 end: [0.99560112482497 -0.0116020271970464 0.0929720023026991 1.35927704162976e+16;0.0111543084687323 0.999923562825107 0.00533384567899673 163727738783727;-0.0930267792081315 -0.00427334436501196 0.995654436477887 -2.82683026995772e+15;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.973806265673;6.41399343134727]
Robot Arm 2 end: [0.998682952801337 0.0376426998115936 -0.0348624000163057 9.73881519504415e+15;-0.0374485932751181 0.999279294694151 0.00620435792603234 -332427916199730;0.0350708232825727 -0.00489063865500125 0.999372862853414 1.18759642239449e+16;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924401e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.099182e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361436263305;-6.64826276794917]
Robot Arm 1 end: [0.995616341886848 -0.0102444128484136 0.0929685526037794 -2.00182421168577e+16;0.00974117639113089 0.999935352554451 0.00586516788535431 -258595025491571;-0.09302262762558 -0.00493383392482714 0.995651770466178 -8.05522997285552e+15;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97252526146293;6.41211338883265]
Robot Arm 2 end: [0.99864653474631 0.0385841178793185 -0.0348764173426839 -2.45101230172413e+16;-0.0384048718461729 0.999245456653195 0.00579509934571794 940260238439972;0.0350737003702951 -0.00444783154161241 0.999374830750161 -1.00305675661209e+16;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213735e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.273063e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.785395759505415;-9.97189296008914;-6.6572402648311]
Robot Arm 1 end: [0.995602726275353 -0.0114692510776884 0.0929713273692507 2.48688483510241e+16;0.0110138338575221 0.999924710285216 0.00541010395339903 409886502976892;-0.0930263774251279 -0.00436234349227892 0.995654088055893 2.73266403871739e+16;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97434354382083;6.41496596652048]
Robot Arm 2 end: [0.998698744917477 0.0372245151300999 -0.0348590357523679 3.99982470171157e+16;-0.0370262498987317 0.999294332046254 0.00631622978268045 -1.5803504869139e+15;0.0350695554390495 -0.00501731138757736 0.999372279417309 -1.03388053178758e+16;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673361e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.746095e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785395759505415;-9.97305746965453;-6.65109586276226]
Robot Arm 1 end: [0.995611977189573 -0.0106573091967368 0.0929688799402963 -1.15380471164721e+16;0.0101660077781687 0.999931754708403 0.00575657985510376 -355792795435052;-0.0930238849034069 -0.00478619749478883 0.995652373648061 -5.83124597836399e+16;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97248544093971;6.41191257878792]
Robot Arm 2 end: [0.998645444323658 0.0386118166111242 -0.0348769859591317 -5.24720345717976e+16;-0.0384331396422328 0.999244461395721 0.00577928604721056 2.2697833261163e+15;0.0350737837828367 -0.00443102561082261 0.99937490247814 4.84442104331244e+16;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183064e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.588299e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.785395759505415;-9.97285991725431;-6.65233929974818]
Robot Arm 1 end: [0.995610412344822 -0.0108003959752522 0.0929691253984455 -3.15127318345468e+16;0.010314157228186 0.999930510178665 0.00570902570554661 -34800493410747.2;-0.0930243247287847 -0.00472506726005994 0.995652624537364 9.14559981044358e+16;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97386287877424;6.41425268595469]
Robot Arm 2 end: [0.998684607518511 0.0375991677145547 -0.0348619748834028 2.53887289856092e+16;-0.0374045637312144 0.999280859883902 0.00621785186109767 -1.42598496034529e+15;0.0350706902936874 -0.00490567598418633 0.999372793818935 -1.11976790963591e+17;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.344629e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.362456e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.785395759505415;-9.97206146908278;-6.65624996197738]
Robot Arm 1 end: [0.995604061803227 -0.0113568552759993 0.0929708231605853 1.17279267160194e+17;0.0108953626556227 0.999925682704389 0.00546993059955002 1.05835410343865e+15;-0.0930260350306261 -0.00443293428796741 0.995653808258714 -1.15018912957547e+17;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97350037998744;6.41343971749429]
Robot Arm 2 end: [0.998674063214756 0.0378754320431833 -0.0348649840050345 6.46237301299492e+16;-0.0376842512956758 0.99927092752481 0.00612459043450016 -1.84383229397537e+15;0.0350715364136426 -0.00480260879608306 0.999373264742628 1.7698263611354e+17;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.852530e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.672028e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.785395759505415;-9.97356419826383;-6.64863342522325]
Robot Arm 1 end: [0.995615952390604 -0.0102821268978015 0.092968560340625 -2.51413565718648e+17;0.00977982249022536 0.999935023536924 0.00585694257016563 -2.68327449181269e+15;-0.0930227413991365 -0.00492204943778941 0.995651818163318 4.88994729821429e+16;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97277205801317;6.412476596591]
Robot Arm 2 end: [0.998653359200389 0.0384101184519124 -0.0348730692410317 -2.44392920361524e+17;-0.0382274856763753 0.999251709197525 0.00588905804752885 8.92262485854332e+15;0.0350731734612403 -0.00454801784678798 0.999374398329787 -2.08236136912193e+17;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.176508e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.796399e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163721579333;-6.65853078357501]
Robot Arm 1 end: [0.99560070973613 -0.01163609283086 0.0929721900169345 4.31429105864263e+17;0.0111904445038063 0.99992326866218 0.00531326074547387 5.87873991411116e+15;-0.0930268817316833 -0.00424948603642506 0.995654529012802 1.04356005122606e+17;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97429065371171;6.41479170169568]
Robot Arm 2 end: [0.998697183098526 0.0372661461374441 -0.0348593003821376 5.01254911371768e+17;-0.0370682341588658 0.999292834896328 0.00630683289800703 -1.91769650312155e+16;0.0350696804578104 -0.00500644354033342 0.999372329532725 3.44859507510268e+16;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.271319e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.465195e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.785395759505415;-9.97346885736037;-6.64904234075487]
Robot Arm 1 end: [0.995615209855998 -0.0103535390465271 0.0929685868054873 -4.78474954606557e+17;0.00985308462763519 0.999934400746747 0.00584045602372397 -7.4277452746376e+15;-0.0930229575251081 -0.00489881949620855 0.995651912538125 -5.0593306352166e+17;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97235390928368;6.41169304229961]
Robot Arm 2 end: [0.998641864984743 0.0387025380690585 -0.0348789198056104 -8.00011252275423e+17;-0.0385257848804128 0.999241201789261 0.00572577908252885 3.32512493715636e+16;0.0350740559265861 -0.00437426494017513 0.999375142980399 3.50991387855223e+17;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.031279e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.232730e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.785395759505415;-9.97227493068446;-6.65538143162157]
Robot Arm 1 end: [0.995605758539417 -0.0112118131529415 0.092970257657895 3.20038604145968e+17;0.010743044478711 0.999926938960072 0.00554109531307226 8.02147610254615e+15;-0.093025590879501 -0.00451796278909452 0.995653467554729 1.12522620021454e+18;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97410031909934;6.41457315836482]
Robot Arm 2 end: [0.998691573782305 0.0374151687286716 -0.0348603729932306 7.06819351067793e+17;-0.0372186298523749 0.999287475967988 0.00627008496276535 -3.2034897898347e+16;0.0350701304265329 -0.00496442570026212 0.999372523351194 -1.19060242781812e+18;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.041627e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.756265e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.785395759505415;-9.97262916365023;-6.65336195104755]
Robot Arm 1 end: [0.995608578375817 -0.0109649837034928 0.0929694992826536 4.42680528368456e+17;0.0104851978405827 0.999929080189947 0.00564758503520836 -1.56787016336237e+15;-0.0930248315812991 -0.0046479805150418 0.995652940028001 -1.87703196798072e+18;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97316941862681;6.41286477602437]
Robot Arm 2 end: [0.998664562424778 0.0381220648312803 -0.0348683227288925 2.53450161184299e+16;-0.0379344961523171 0.999262061912924 0.00602541477428383 7.83123566724385e+15;0.035072293318179 -0.00469465595459104 0.999373751129612 2.31359328089547e+18;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.761691e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.761155e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.785395759505415;-9.97324708416375;-6.65038084070059]
Robot Arm 1 end: [0.995613472329001 -0.0105182202909941 0.0929687084932307 -2.17833341707734e+18;0.01002246523316 0.999932965617425 0.00579779804671887 -1.72044307240018e+16;-0.0930234589103162 -0.00484059019651088 0.995652150491782 2.1594176303196e+18;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97308048640945;6.41293117655808]
Robot Arm 2 end: [0.998662034130993 0.0381873333213084 -0.0348693154385594 -2.03101638441128e+18;-0.0380008131152708 0.999259715969048 0.00599651932675273 6.75747116711845e+16;0.0350724933234683 -0.00466343384912257 0.999373890291722 -3.30917732561502e+18;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.312919e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.431925e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.785395759505415;-9.97175093531609;-6.65793527848102]
Robot Arm 1 end: [0.995601604589413 -0.0115624756170411 0.0929717919403081 4.98030810458604e+18;0.0111124026231146 0.999923904482216 0.00535721503975555 5.72027161924135e+16;-0.0930266598719336 -0.00430051190507802 0.995654330654181 -1.7760258397521e+18;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97415185197379;6.414478269867]
Robot Arm 2 end: [0.998693090581963 0.0373749483187274 -0.0348600639990792 5.65737474781407e+18;-0.037178021979375 0.999288922271235 0.00628048626255651 -2.09298368438483e+17;0.0350700086334257 -0.00497625001034994 0.999372468817451 2.3567436698419e+18;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.280231e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.538009e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362749625136;-6.64831780135496]
Robot Arm 1 end: [0.99561644371951 -0.0102345233487461 0.092968551280206 -7.96343552793198e+18;0.00973104758848425 0.999935438843055 0.00586727011289942 -9.81848849503355e+16;-0.0930225978359396 -0.00493686920740656 0.995651758203782 -2.00042672329771e+18;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97233781641119;6.41165491461987]
Robot Arm 2 end: [0.998641429481801 0.0387135552874288 -0.0348791622541749 -1.06220473169315e+19;-0.0385370420937821 0.999240805958778 0.00571909901251297 4.28088031870758e+17;0.0350740888578439 -0.00436718946921891 0.999375172768932 9.16609908124214e+17;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.480564e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.026110e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.785395759505415;-9.97180145232356;-6.65780304278897]
Robot Arm 1 end: [0.995602003084068 -0.011529477700841 0.0929716225465382 1.09027911256905e+19;0.011077480555294 0.999924189625572 0.00537628367832233 1.69575223806546e+17;-0.0930265600758045 -0.00432274745833249 0.995654243688477 8.97423858195061e+18;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97426145739704;6.41476280534078]
Robot Arm 2 end: [0.998696321470547 0.0372890874030541 -0.0348594526899956 1.2565489561132e+19;-0.0370913757109755 0.999292009888313 0.00630149357246397 -5.28072860262647e+17;0.0350697494867855 -0.00500029339378646 0.999372357901153 -1.10389943400216e+19;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.497092e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.751466e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.785395759505415;-9.97317561661089;-6.65059885830373]
Robot Arm 1 end: [0.995612909712852 -0.0105708374001504 0.0929687657752242 -7.30259225190732e+18;0.0100767140354428 0.999932507401672 0.00578277404473732 -1.66268475020355e+17;-0.0930236198358074 -0.00482058482594788 0.995652232516243 -2.1343919653761e+19;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97285083026922;6.41232717554733]
Robot Arm 2 end: [0.998655560150044 0.0383537648255831 -0.0348720648242378 -9.16715399958957e+18;-0.0381701024275683 0.999253734396554 0.00591756497309216 4.64012253475732e+17;0.0350730018970563 -0.00457853887672567 0.999374264987689 2.68927565564985e+19;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.274507e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.725815e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.785395759505415;-9.97272188372313;-6.65316059382717]
Robot Arm 1 end: [0.995609315865785 -0.0108991929349061 0.0929693377121228 -6.85934333249977e+18;0.0104167448629152 0.999929651596424 0.0056730313514824 5.22651782775144e+16;-0.0930246289308589 -0.00467968499171316 0.995652810451843 3.63564671805158e+19;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97341201027412;6.41341687402429]
Robot Arm 2 end: [0.998671513268823 0.0379418390585035 -0.0348658204294395 -1.10232296014634e+19;-0.0377515777259952 0.999268540281927 0.00609940833395409 2.56408042961682e+17;0.0350717402556168 -0.00477506562099183 0.999373389571564 -4.55407609674654e+19;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.044593e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.045010e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.785395759505415;-9.97218739585931;-6.65570603079299]
Robot Arm 1 end: [0.995605062264027 -0.0112716416441718 0.092970479663647 3.82704888204913e+19;0.0108057994136848 0.999926420582225 0.00551254212221041 2.98441251743119e+17;-0.0930257743492332 -0.00448369448817693 0.995653605321882 -4.93264748947674e+19;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97393376718203;6.41404373790764]
Robot Arm 2 end: [0.998686683088593 0.0375444697734406 -0.0348614659894676 5.60844153119095e+19;-0.0373492624609241 0.999282826595412 0.00623418520006413 -1.98127003253074e+18;0.0350705234510208 -0.00492394769619884 0.99937270981549 4.97305539178125e+19;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.287959e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.778185e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.785395759505415;-9.97351166833506;-6.64903045036329]
Robot Arm 1 end: [0.995615543634066 -0.0103215166802844 0.0929685730107824 -9.23501000709724e+19;0.00982021884320103 0.999934679981845 0.00584799721928934 -9.36576845304598e+17;-0.0930228605027506 -0.00490938519814727 0.995651869561275 3.17969101535217e+19;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97243925012752;6.41179780378516]
Robot Arm 2 end: [0.998644183370558 0.0386438116181688 -0.0348776553651677 -1.26155536645217e+20;-0.0384658025434938 0.999243311793567 0.00576071788144873 4.92554238092385e+18;0.0350738799512808 -0.00441131039989097 0.99937498632181 -3.04300673694851e+19;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.768295e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.036068e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.785395759505415;-9.97162998424949;-6.65866108782289]
Robot Arm 1 end: [0.995600652943041 -0.0116407426765193 0.0929722161149721 1.68520906926549e+20;0.0111953800577715 0.999923228517567 0.0053104177206444 2.2136451828702e+18;-0.0930268957063068 -0.00424619605585348 0.995654541743422 1.64397347188264e+19;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97433790793081;6.41480710486934]
Robot Arm 2 end: [0.998698578436902 0.0372289556235493 -0.034859063258956 1.86217186326783e+20;-0.0370307274464669 0.999294172353961 0.00631524537910866 -7.48311351694515e+18;0.0350695687583634 -0.00501617011201448 0.999372284678993 -7.96702037229233e+19;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.691429e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.356023e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.785395759505415;-9.97353142050743;-6.6488373115566]
Robot Arm 1 end: [0.995615697438421 -0.0103067179325438 0.0929685676971842 -2.06024072018945e+20;0.00980503794290709 0.999934809045732 0.00585140065404298 -2.99148446853381e+18;-0.0930228157235906 -0.00491418600939802 0.995651850061416 -1.65996606826972e+20;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97258388778215;6.41188478030168]
Robot Arm 2 end: [0.998648145699514 0.0385431446437649 -0.0348755945023399 -2.15392445214871e+20;-0.0383630721703042 0.999246928922973 0.005818052222494 9.58416941138981e+18;0.0350735767291826 -0.00447225211470104 0.9993747261045 2.74016084915993e+20;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.125952e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.112362e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.785395759505415;-9.9721430806182;-6.65615720124416]
Robot Arm 1 end: [0.995604710002469 -0.0113017458375875 0.0929705972978628 1.83572724739062e+20;0.0108374155697687 0.999926159844679 0.00549775245356446 3.74689062951731e+18;-0.0930258665354263 -0.00446602723850986 0.995653676112351 4.08097842520071e+20;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97372799019998;6.41387247483157]
Robot Arm 2 end: [0.998680669443363 0.0377026610837151 -0.0348630151788694 4.21813936838259e+19;-0.0375092650963592 0.999277138860902 0.00618504502006556 -3.8442584723608e+18;0.0350710067161838 -0.00486919882277381 0.999372961106482 -5.63192781924256e+20;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.954748e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.794698e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.785395759505415;-9.97276979214773;-6.65274644053131]
Robot Arm 1 end: [0.995609696654983 -0.0108650165072774 0.0929692602039469 7.10769095259097e+19;0.0103812293066845 0.999929948533702 0.00568578081449927 -1.855710806614e+18;-0.0930245236733553 -0.00469568330332035 0.995652744963654 -7.28488120379633e+20;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97365075212314;6.41351845071178]
Robot Arm 2 end: [0.998678421845226 0.0377615600946918 -0.0348636532787818 4.48812774418941e+20;-0.0375688910342572 0.999275021294079 0.00616524485930303 -1.43927690143147e+19;0.0350711871367972 -0.00484730821529147 0.999373061191806 8.0172755674766e+20;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.454244e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.255943e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.785395759505415;-9.97313630914675;-6.65107205591915]
Robot Arm 1 end: [0.995612599782215 -0.010599678509466 0.0929688010592584 -7.06034480580284e+20;0.0101064768168738 0.999932256306276 0.00577424666865245 -4.75498425915016e+18;-0.0930237081675956 -0.00480932570496291 0.995652278712309 9.35439905354987e+20;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97264507812068;6.41209934051928]
Robot Arm 2 end: [0.99864983401075 0.0385001372716008 -0.0348747539112532 -1.33848811898033e+21;-0.0383192167290889 0.999248474333921 0.0058415897323733 5.01703859455568e+19;0.0350734466451714 -0.00449732936309455 0.999374618133275 -8.30784236710537e+20;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.664896e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.092364e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.785395759505415;-9.9718309540204;-6.65762292500447]
Robot Arm 1 end: [0.995602236056103 -0.0115101243096613 0.0929715257402186 1.78579999000912e+21;0.0110570153211044 0.999924356904206 0.00538728891926467 1.93832117733513e+19;-0.0930265014513437 -0.00433560930976412 0.995654193241628 -9.28292028522908e+20;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97432668479621;6.41470240134146]
Robot Arm 2 end: [0.998698246951203 0.0372377952108219 -0.0348591185263094 2.45103520203675e+21;-0.0370396412402819 0.999293854459353 0.00631327304656788 -9.51891256126191e+19;0.0350695952840293 -0.00501388547996465 0.999372295212854 -2.07566880602082e+20;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.393394e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.325129e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362480180752;-6.64845585210596]
Robot Arm 1 end: [0.995616422832785 -0.0102365527610398 0.0929685515280289 -3.10735489528217e+21;0.00973312593549606 0.99993542113545 0.00586684056319373 -3.66327867115177e+19;-0.0930226039474981 -0.00493624819479426 0.99565176071184 -3.40245083208925e+20;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9724026740533;6.41158536120617]
Robot Arm 2 end: [0.998643187919261 0.038669042946783 -0.0348781929009382 -3.59207548469625e+21;-0.0384915686586441 0.999242405235601 0.00574584381679212 1.51843150518232e+20;0.0350739556459218 -0.00439553142975156 0.999375053190142 2.30490717575754e+21;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.770711e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.781070e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.785395759505415;-9.97172783314565;-6.65827327764423]
Robot Arm 1 end: [0.995601422543997 -0.0115775059344558 0.0929718709325997 4.61105473826769e+21;0.0111283213941333 0.999923774631027 0.00534840075046698 6.84146444813473e+19;-0.0930267052588621 -0.00429025453514845 0.995654370665194 2.80012197258799e+21;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97399634023773;6.41424586487598]
Robot Arm 2 end: [0.998688518300363 0.0374960186220238 -0.0348610384601798 2.5702079442821e+21;-0.0373002968735988 0.999284568749401 0.00624808070245416 -1.24432191380877e+20;0.0350703759342083 -0.00493955937506839 0.999372637950937 -6.30365721535181e+21;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.770570e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.271003e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.785395759505415;-9.97328232426335;-6.65016186088909]
Robot Arm 1 end: [0.995613749301602 -0.0104921918343524 0.0929686835021243 -3.83217348912555e+21;0.00999565318309248 0.99993319234412 0.00580497768650624 -7.36388253893842e+19;-0.0930233794217903 -0.0048502328818987 0.995652110991555 -7.65681831170086e+21;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97332740464622;6.41294662373433]
Robot Arm 2 end: [0.998669080586154 0.038005046786009 -0.0348666588591388 1.93165926400028e+21;-0.0378156965586668 0.999266268141223 0.0060744093454631 -3.31252321833249e+19;0.0350719342920965 -0.00474781780470583 0.99937351258231 1.1112295617674e+22;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.800077e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.719368e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.785395759505415;-9.97258045576842;-6.65399576314064]
Robot Arm 1 end: [0.995608190746127 -0.0109993546232997 0.0929695902814191 -5.77068010969094e+20;0.0105210043698833 0.999928781781713 0.00563381146183491 4.50565193970269e+19;-0.0930249374429939 -0.00463093537090518 0.995653009562728 1.41411201800982e+22;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9729287722279;6.41251759756151]
Robot Arm 2 end: [0.998657748031742 0.038297631213783 -0.0348711017833373 -1.22806664709734e+22;-0.0381129747791128 0.999255751769582 0.00594505751816331 4.31023091450569e+20;0.0350728306479176 -0.00460803633021994 0.999374135422526 -1.4593259912108e+22;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.252123e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.357760e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.785395759505415;-9.97232185839645;-6.65511934693947]
Robot Arm 1 end: [0.995606132006366 -0.0111795429593147 0.0929701442966658 1.18857499833626e+22;0.0107092384273787 0.999927218666792 0.00555604010045188 7.21954051829137e+19;-0.0930254917946021 -0.00453598815178236 0.99565339485579 -2.05245190477298e+22;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.974228183293;6.41445341640792]
Robot Arm 2 end: [0.998695339989143 0.0373151974894281 -0.0348596316718096 2.91081435027212e+22;-0.0371177186339116 0.999291070936471 0.0062952768051846 -1.09490712049287e+21;0.0350698281630097 -0.00499315360920358 0.999372390838196 7.10423320177646e+21;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.299201e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.554464e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.785395759505415;-9.97344401124045;-6.64950499691279]
Robot Arm 1 end: [0.995615015884055 -0.0103720903674297 0.0929685962441196 -3.31905033719701e+22;0.00987213493039059 0.999934239004733 0.00583597617725753 -3.17360018465533e+20;-0.0930230138089948 -0.00489258698800827 0.995651937925325 1.69414977736878e+22;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97233058117873;6.41145980757207]
Robot Arm 2 end: [0.998641233859181 0.0387185026042662 -0.0348792716709592 -5.06406535790387e+22;-0.0385420976372061 0.99924062821101 0.00571608626512909 2.07022934492497e+21;0.0350741036369742 -0.00436399914639729 0.999375186186606 1.24396907626011e+22;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.090327e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.380473e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164480226432;-6.65867921538385]
Robot Arm 1 end: [0.995600769331655 -0.0116312106809892 0.0929721627386954 6.44995749547201e+22;0.0111852631786403 0.999923310814387 0.00531623716737857 8.15769090247243e+20;-0.0930268670537742 -0.00425293170527196 0.995654515672012 -2.70674666160685e+21;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97419519044387;6.41450022589658]
Robot Arm 2 end: [0.998694367346476 0.0373410490076689 -0.0348598148220725 5.29633108278404e+22;-0.0371438058280483 0.999290141296242 0.00628897422029815 -2.29271577853929e+21;0.0350699061736778 -0.00498593693724618 0.999372424131178 -6.28054640359673e+22;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.166395e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.339591e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357870156307;-6.64871271831634]
Robot Arm 1 end: [0.995616065087233 -0.0102712329166071 0.0929685576664963 -8.61116412046667e+22;0.00976865610537668 0.999935118568243 0.00585935250752386 -1.18081009638643e+21;-0.0930227085077118 -0.00492548761904303 0.99565180423359 -5.19481477471627e+22;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97299799509951;6.41238381368723]
Robot Arm 2 end: [0.998659699287879 0.0382474722340921 -0.0348702722378944 -2.309181338909e+22;-0.0380619537131918 0.99925755448856 0.00596887569625598 1.42891314268003e+21;0.0350726773682498 -0.0046336449200246 0.999374022394508 1.36585183059766e+23;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.104760e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.985896e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.785395759505415;-9.97202085666691;-6.65688112553901]
Robot Arm 1 end: [0.995603739540583 -0.0113841172527609 0.0929709400145094 9.20424345649964e+22;0.0109240623048903 0.999925446752369 0.00545580423351674 1.66567489043215e+21;-0.0930261182440986 -0.0044161987558279 0.995653874854602 1.44272922629247e+23;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97325431049829;6.41299634042225]
Robot Arm 2 end: [0.998666986108403 0.0380593534196814 -0.0348674127866587 -8.67125042318302e+22;-0.0378708145048933 0.999264316042858 0.00605211468285218 2.59237681464601e+21;0.0350721009620955 -0.00472358980799731 0.999373621541729 -2.13432458750339e+23;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.823265e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.083943e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.785395759505415;-9.97290602225572;-6.65215437441998]
Robot Arm 1 end: [0.995610778081238 -0.0107671762035279 0.0929690619780209 -1.02748216151166e+22;0.010279717599154 0.999930799005611 0.00572054246394836 -1.16136794235913e+21;-0.0930242225151736 -0.00473973803098609 0.995652564356083 -2.77569854735581e+23;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97404649752424;6.41407340897648]
Robot Arm 2 end: [0.998689991329354 0.0374570699081982 -0.0348607104412382 3.0856452104879e+23;-0.037260948103822 0.999285969259567 0.00625886470315854 -1.11729987034065e+22;0.0350702575550819 -0.00495172241351566 0.999372581913352 1.96851213159502e+23;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.180253e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.856411e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.785395759505415;-9.97301470348317;-6.65181617627656]
Robot Arm 1 end: [0.995611638971352 -0.0106884473592494 0.0929689272705001 -2.1576798567024e+23;0.0101982054964102 0.9999314837733 0.00574668283269565 -1.07643452424076e+21;-0.0930239805073507 -0.00477334808862328 0.995652426401198 3.92577488133028e+23;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97237694989884;6.4115182377021]
Robot Arm 2 end: [0.998642489441794 0.0386867327981506 -0.0348785748977705 -6.33283444975102e+23;-0.038509637749672 0.999241769652802 0.00573529262767447 2.51154255662354e+22;0.035074008637222 -0.00438434562284006 0.999375100466074 -5.33321296798097e+22;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.634713e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.105983e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.785395759505415;-9.97192811744226;-6.65722394050468]
Robot Arm 1 end: [0.995603004544145 -0.0114459588287389 0.0929712179610511 6.25146580738675e+23;0.0109892501420307 0.999924911723796 0.00542284938699176 6.322772407066e+21;-0.0930263066233759 -0.00437732117269613 0.995654028935435 -4.4216168075765e+23;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97431247229681;6.41461455389623]
Robot Arm 2 end: [0.998697827246942 0.0372489833802293 -0.0348591894537995 8.44565318452162e+23;-0.0370509241796231 0.999293452105778 0.00631075240779397 -3.45968138494589e+22;0.0350696288784513 -0.00501096953254141 0.999372308659126 -5.23494892016413e+23;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.897878e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.167287e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360629777569;-6.64867653087839]
Robot Arm 1 end: [0.99561627932574 -0.0102504824467383 0.0929685535607542 -1.19017819126577e+24;0.0097473939072743 0.999935299598117 0.00586386644130618 -1.34186062848509e+22;-0.093022645918003 -0.00493195977621013 0.995651778042494 3.41128887609742e+22;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97270182126532;6.41188946277579]
Robot Arm 2 end: [0.998651405753836 0.0384600380903158 -0.0348739911105605 -8.2447895081866e+23;-0.0382783444325963 0.99924991528804 0.00586303200952502 3.82752470757929e+22;0.0350733250973944 -0.00452020651482257 0.999374519186714 1.48703707436261e+24;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.379776e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.860052e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.785395759505415;-9.97167388818692;-6.65864231895219]
Robot Arm 1 end: [0.995600997955901 -0.0116124542506119 0.092972059111821 1.89438241676491e+24;0.0111653650146069 0.999923472775189 0.00532759019604575 2.69809249792894e+22;-0.0930268106155698 -0.00426608713974013 0.995654464664931 8.09229625096413e+23;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97358219798198;6.41347348205811]
Robot Arm 2 end: [0.998676431836349 0.037813607103246 -0.0348642454738214 -2.03797730366601e+23;-0.0376216046159907 0.99927315013586 0.00614705487791904 -2.8054246558758e+21;0.0350713467197311 -0.00482726997333146 0.999373152583092 -2.77048178931522e+24;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.310468e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.095880e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.785395759505415;-9.97337617242628;-6.64979235819222]
Robot Arm 1 end: [0.995614485346293 -0.0104226143478304 0.0929686273900593 -1.83156036274423e+24;0.00992405672916919 0.999933798600793 0.00582335932471655 -3.13950383329423e+22;-0.0930231673652941 -0.00487519496481227 0.995652008890347 -2.68550537147502e+24;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97379190308976;6.41358654763634]
Robot Arm 2 end: [0.998682533431361 0.0376537219916495 -0.0348625105133586 2.78282852834666e+24;-0.0374597438558771 0.999278898399657 0.00620087118230846 -9.45567821995009e+22;0.0350708569808401 -0.00488676102782609 0.999372880639297 3.51349470682637e+24;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.975095e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.473299e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.785395759505415;-9.97243861294199;-6.65482956770248]
Robot Arm 1 end: [0.99560706150099 -0.0110986764872617 0.0929698793674295 5.67809601415966e+23;0.0106246514619378 0.999927919904858 0.00559211731423003 2.66135361363298e+22;-0.0930252431906289 -0.00457977892205203 0.995653217618639 5.38443590639886e+24;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97253577286827;6.41174857696995]
Robot Arm 2 end: [0.998646823098209 0.0385767884544147 -0.0348762685560953 -7.10105398793404e+24;-0.0383973932465922 0.999245720014153 0.00579924350788838 2.71856617382191e+23;0.0350736782747418 -0.0044522383068035 0.999374811903091 -3.04081423623839e+24;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.050431e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.593628e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724616911209;-6.65450605335018]
Robot Arm 1 end: [0.995607245250791 -0.0110825960434468 0.0929698298862063 3.42632431770506e+24;0.0106078526913379 0.999928059400265 0.00559906110862817 1.14581802643836e+22;-0.0930251937133766 -0.0045882555461805 0.995653183214734 -8.32961566143962e+24;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97434300334347;6.41458047741957]
Robot Arm 2 end: [0.998698728951568 0.037224941014452 -0.0348590383830747 1.17144135267244e+25;-0.037026679330478 0.999294316730271 0.00631613555417633 -4.62049860092902e+23;0.0350695567163325 -0.00501720211386278 0.999372279921086 -2.82640032322778e+24;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.981206e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.484093e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.785395759505415;-9.97336178709026;-6.65005354797757]
Robot Arm 1 end: [0.9956143726768 -0.0104333036043414 0.0929686350081659 -1.1632577479159e+25;0.00993504904733726 0.999933705443471 0.00582061152431004 -1.03447593859047e+23;-0.0930231999009329 -0.00487143654270117 0.995652024246524 8.18597609414272e+24;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97247624123944;6.41151717090253]
Robot Arm 2 end: [0.998645192845765 0.0386182005693719 -0.0348771184421616 -1.554901932395e+25;-0.0384396559565926 0.999244232013478 0.00577560713114277 6.7091602927541e+23;0.0350738029871813 -0.00442711786360742 0.999374919122668 1.38755047507432e+25;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.582838e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.126796e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.785395759505415;-9.97168130140373;-6.65858726792825]
Robot Arm 1 end: [0.99560105626084 -0.011607663968989 0.092972032942616 2.42014758968432e+25;0.0111602850773715 0.999923514143791 0.00533046895913756 2.93806854490961e+23;-0.0930267961895477 -0.0042694261342206 0.995654451700585 -4.41104585657355e+24;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97387654150714;6.41389149421162]
Robot Arm 2 end: [0.998685007250836 0.0375886416313753 -0.0348618747732612 7.91823690535361e+24;-0.0373939196716191 0.999281238353484 0.00622104865123973 -4.35305699598213e+23;0.0350706581630713 -0.00490924587249778 0.999372777416402 -3.25287933617426e+25;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.296617e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.178007e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361040725388;-6.64867010264281]
Robot Arm 1 end: [0.995616311206782 -0.0102473899623982 0.0929685530594065 -3.51197224630428e+25;0.00974422595856631 0.999935326579332 0.00586453059944757 -4.57633868674874e+23;-0.0930226365970647 -0.00493291573432947 0.995651774177542 -1.51394872634907e+25;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9734839636044;6.41303046822493]
Robot Arm 2 end: [0.998673588841019 0.0378877977586809 -0.0348651363455773 1.7972941496841e+25;-0.0376967853119339 0.999270482986537 0.00611998439529782 -5.02517933885968e+23;0.0350715743664918 -0.00479756322001257 0.999373287645115 5.19675516023591e+25;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.797172e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.994743e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.785395759505415;-9.97191119270967;-6.657538662821]
Robot Arm 1 end: [0.995602870558585 -0.0114571822218806 0.0929712703529428 4.27695176205648e+25;0.0110010936800833 0.999924814655124 0.00541673100299973 7.13494903700382e+23;-0.0930263407500659 -0.00437012728092113 0.995654057348335 4.95138274411939e+25;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97278652465943;6.41211771211183]
Robot Arm 2 end: [0.998653762617935 0.0383997979092521 -0.0348728824385013 -7.02958859169279e+25;-0.0382169741654527 0.999252080084153 0.00589434755764876 2.5581736182569e+24;0.0350731420702231 -0.0045536763193967 0.999374373664494 -6.18618744956404e+25;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.178170e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.625040e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.785395759505415;-9.97303524657683;-6.65159921800714]
Robot Arm 1 end: [0.995611801478549 -0.0106735008524501 0.0929689041356836 -1.79586402900975e+25;0.0101827475031957 0.999931613816666 0.00575146444692658 -6.03066678554575e+23;-0.0930239346078383 -0.00477954700268192 0.99565240095152 -1.03857559966551e+26;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97428568862358;6.41439889352521]
Robot Arm 2 end: [0.998697036543858 0.0372700495220801 -0.034859325970796 1.46047784890508e+26;-0.0370721713388878 0.999292694523069 0.00630593252773751 -5.57853944157543e+24;0.0350696921962064 -0.00500540522294948 0.999372334321807 1.2456062645087e+25;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.204101e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.957035e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.785395759505415;-9.9728842116528;-6.65260279397473]
Robot Arm 1 end: [0.995610605100308 -0.0107829047578566 0.092969091513152 -5.99986952240636e+25;0.0102960203254864 0.999930662246454 0.00571512596749727 -1.15700343122586e+23;-0.093024270904184 -0.00473282836686303 0.995652592704099 1.60426399440952e+26;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97235026847148;6.41130648646674]
Robot Arm 2 end: [0.998641766410424 0.0387050321706285 -0.0348789745453995 -2.35100764704093e+26;-0.038528333190066 0.999241112179992 0.00572427033668752 9.75532599147291e+24;0.0350740633839776 -0.00437266668782743 0.999375149712946 9.8503426680086e+25;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.979716e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.600845e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.785395759505415;-9.97204005718098;-6.65675040958764]
Robot Arm 1 end: [0.995603891870867 -0.0113712420394786 0.0929708843996998 2.127821576771e+26;0.0109105052851333 0.999925558180195 0.00546250605929491 1.95979909724187e+24;-0.0930260789564185 -0.00442413296639577 0.995653843301722 -1.98751246818657e+26;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97411043454724;6.41420589439001]
Robot Arm 2 end: [0.998691871393952 0.03740728152466 -0.0348603112519357 2.115835381046e+26;-0.0372106656757028 0.999287759584855 0.00627215304748745 -9.5386231942249e+24;0.0350701065241906 -0.0049667728773178 0.99937251252752 -3.4386085945558e+26;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.914875e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.213595e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357207933221;-6.64897894448911]
Robot Arm 1 end: [0.995616013638462 -0.0102762081163875 0.0929685588433359 -4.47435181636028e+26;0.00977375538927517 0.999935075167361 0.00585825534055595 -4.81677001286929e+24;-0.0930227235262908 -0.00492392087601234 0.995651810579862 7.54776091055551e+25;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97315269548221;6.41245599190404]
Robot Arm 2 end: [0.9986640861385 0.038134371949292 -0.0348685063228567 -4.20764418433877e+24;-0.0379469979306839 0.999261619550676 0.00602005323966964 2.74060877632296e+24;0.0350723310488868 -0.00468885582982032 0.999373777035302 6.75257306844612e+26;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.893738e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.109227e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164092256793;-6.65890392842791]
Robot Arm 1 end: [0.995600738852788 -0.0116337079189459 0.0929721766758141 7.59283570146613e+26;0.0111879133427474 0.999923289253121 0.00531471582068238 1.04146762772182e+25;-0.0930268745622326 -0.00425117034192838 0.995654522492569 2.04106902495173e+26;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97309706773204;6.41257534850441]
Robot Arm 2 end: [0.998662504668475 0.0381751978537666 -0.0348691272921249 -5.76873744365323e+26;-0.0379884796822403 0.999260152146156 0.00600197836319719 1.90524975651286e+25;0.0350724561546614 -0.00466932561148216 0.999373864085715 -9.74249277202064e+26;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.015071e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.066945e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.785395759505415;-9.97345609512912;-6.64949601796675]
Robot Arm 1 end: [0.995615110245397 -0.010363071043502 0.0929685915220508 -8.26214005587933e+26;0.009862872057284 0.99993431763849 0.00583816440572414 -1.29992186321624e+25;-0.0930229864379136 -0.0048956273749079 0.995651925537624 -9.17205893372535e+26;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97414266977371;6.41407881775985]
Robot Arm 2 end: [0.998692820204298 0.0373821219598772 -0.0348601180457416 1.63901371178158e+27;-0.0371852638076078 0.999288664308791 0.00627865746082964 -6.05216860741554e+25;0.0350700303385209 -0.00497416744075745 0.999372478423499 7.14960441220949e+26;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.295636e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782156e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.785395759505415;-9.97229949851808;-6.65564607699038]
Robot Arm 1 end: [0.995605954045385 -0.0111949357753839 0.0929701978182445 5.14200317459579e+26;0.0107253601579761 0.999927085238182 0.00554895094183451 1.36082718925603e+25;-0.0930255390678291 -0.00452742974083723 0.995653429392518 2.0063851253377e+27;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9723402020672;6.41127771354889]
Robot Arm 2 end: [0.998641494007983 0.0387119232116885 -0.0348791262326888 -3.10282718123254e+27;-0.038535374377149 0.999240864596341 0.00572009111554994 1.24859780853089e+26;0.0350740839811459 -0.00436824015017008 0.999375168348141 2.164634689968e+26;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663433e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.477789e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.785395759505415;-9.97260369306175;-6.65388250254672]
Robot Arm 1 end: [0.995608375688815 -0.0109829737013396 0.0929695463415579 8.65103262709102e+26;0.0105039353767955 0.999928923991301 0.00564041728941143 -1.70106921526264e+24;-0.0930248869920277 -0.0046391005889363 0.995652976265248 -3.3079727122552e+27;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97426742691893;6.41438842283789]
Robot Arm 2 end: [0.99869649760846 0.0372843991367725 -0.0348594211886156 3.71525461826915e+27;-0.0370866461883953 0.999292178485729 0.00630259404863752 -1.55710161412217e+26;0.0350697353724297 -0.00500155958226778 0.999372352060359 -3.16041034792978e+27;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.460918e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.062487e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.785395759505415;-9.97326581354182;-6.65067125457295]
Robot Arm 1 end: [0.995613619572434 -0.0105043934768431 0.0929686949438422 -3.95463641020046e+27;0.0100082202273284 0.999933086054225 0.00580163269738187 -3.19144105394597e+25;-0.0930234166742913 -0.00484573335602486 0.995652129419951 3.7238741113522e+27;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97283580322264;6.4119218830529]
Robot Arm 2 end: [0.998655139477226 0.0383645447671392 -0.0348722540213904 -2.8094201442829e+27;-0.0381810768510013 0.999253346987874 0.00591218267898963 1.40556853610207e+26;0.0350730347449453 -0.00457277140714288 0.999374290241368 7.80146290410941e+27;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.699291e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.912687e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.785395759505415;-9.97173858609462;-6.65838997009307]
Robot Arm 1 end: [0.99560150726192 -0.0115705147615661 0.0929718340463665 8.89997834739105e+27;0.0111209160256973 0.999923835027439 0.00535251069760659 1.03158660625953e+26;-0.0930266841532156 -0.00429503575898796 0.995654352023476 -2.92817471087413e+27;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97342869115532;6.41306093650253]
Robot Arm 2 end: [0.998671993913175 0.0379293338126078 -0.0348656594662236 -3.00402389070016e+27;-0.0377388964027062 0.999268989821173 0.00610423460217504 6.62596017395241e+25;0.0350717018661586 -0.0047803366308601 0.999373365719693 -1.33278264436385e+28;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.460279e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.651594e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362637444889;-6.64871024553175]
Robot Arm 1 end: [0.995616435023855 -0.0102353683050656 0.092968551381897 -1.4020524056838e+28;0.00973191290821946 0.999935431470375 0.00586709138339943 -1.74290446252257e+26;-0.0930226003804215 -0.00493661076184738 0.995651759247504 -3.89112288969846e+27;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97392088582609;6.41363886646097]
Robot Arm 2 end: [0.998686305644038 0.0375544244504995 -0.0348615565237006 1.61191771493459e+28;-0.0373593251513941 0.999282468661859 0.00623126386002362 -5.67681486676931e+26;0.0350705537922608 -0.00492067365840756 0.999372724876686 1.47851654936083e+28;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.942754e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.028865e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.785395759505415;-9.97181674037473;-6.65811675195156]
Robot Arm 1 end: [0.995602123789829 -0.0115194561733769 0.0929715721855363 1.88899733156912e+28;0.0110668817672139 0.999924276241635 0.00538199875434565 2.96598055583249e+26;-0.0930265297274452 -0.00432942399297142 0.995654217514674 1.63850899244413e+28;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9724473486097;6.41142933198096]
Robot Arm 2 end: [0.998644404142333 0.038638212579893 -0.0348775372386928 -3.66504595598712e+28;-0.0384600858018115 0.999243512968678 0.00576399081661014 1.42843610691509e+27;0.0350738631365679 -0.00441478409977951 0.999374971572748 -9.47125528828477e+27;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.949025e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.650713e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.785395759505415;-9.973155210024;-6.65109267909223]
Robot Arm 1 end: [0.995612748853064 -0.0105858191865901 0.0929687837577452 -1.20281085076826e+28;0.0100921721675709 0.999932376961824 0.00577837039453201 -2.85250868815299e+26;-0.0930236657103218 -0.00481476226049834 0.995652256403906 -3.81447805092402e+28;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97433948158014;6.4144251980739]
Robot Arm 2 end: [0.998698624920204 0.0372277158555498 -0.0348590555619776 5.467896722925e+28;-0.0370294773180776 0.999294216939371 0.00631552064982684 -2.19311473881873e+27;0.0350695650392841 -0.00501648918137536 0.999372283207939 -2.23608593584629e+28;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.167521e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.613623e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.785395759505415;-9.97274715575063;-6.65341970113712]
Robot Arm 1 end: [0.995609516761279 -0.0108811799110221 0.0929692963193541 -1.38275055155552e+28;0.0103980223376323 0.999929808091036 0.00567979070827207 7.15569611983498e+25;-0.0930245734515241 -0.00468815686252535 0.99565277578049 6.40280902884986e+28;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97257245591167;6.4114857005381]
Robot Arm 2 end: [0.998647831057356 0.0385511521800476 -0.0348757536123589 -6.42038199716127e+28;-0.0383712397928842 0.999246641188722 0.00581360729795096 2.84682981591442e+27;0.0350736009157334 -0.00446752041390057 0.999374746419058 7.88843465082567e+28;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.559897e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.806406e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.785395759505415;-9.97216405879882;-6.65621616193646]
Robot Arm 1 end: [0.995604876735361 -0.0112875105722049 0.0929705411721828 6.99462682646341e+28;0.0108224620220915 0.999926283130546 0.00550478160427449 5.62886837122859e+26;-0.093025822955492 -0.00447441725956955 0.995653642515127 -8.57575393463447e+28;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97374294529577;6.41351350290972]
Robot Arm 2 end: [0.998681105274249 0.0376912258596221 -0.0348628951953625 1.48865986419352e+28;-0.0374976920797344 0.999277549994371 0.00618879357712315 -1.22100427325063e+27;0.0350709717130464 -0.00487335310087177 0.999372942085515 -1.63994629533599e+29;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.084885e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.196182e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735223402095;-6.64936159462148]
Robot Arm 1 end: [0.995615626748304 -0.0103135229687768 0.0929685700575926 -1.64954293251345e+29;0.00981201811086912 0.999934749695698 0.00584984201444722 -1.68912080227212e+27;-0.0930228363100855 -0.00491198483044573 0.995651858999848 5.21833947782634e+28;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97363516189447;6.41311006848722]
Robot Arm 2 end: [0.998677968872892 0.0377734156022145 -0.0348637858193553 1.26976991584458e+29;-0.0375808963675554 0.999274595067892 0.00616115899100084 -4.03757836341972e+27;0.0350712234763284 -0.00484280142517784 0.999373081765879 2.36164722199402e+29;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.218824e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.417039e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.785395759505415;-9.97162972978945;-6.6590544235482]
Robot Arm 1 end: [0.99560065094489 -0.0116409062236754 0.0929722170350033 2.97689574177384e+29;0.0111955536677037 0.99992322710561 0.00531031757768223 3.93642708411066e+27;-0.0930268961977436 -0.00424608019164374 0.995654542191628 3.66301305898719e+28;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97265783888083;6.41173790560101]
Robot Arm 2 end: [0.998650186965643 0.0384911376289059 -0.0348745809655052 -3.86445934502187e+29;-0.0383100421039607 0.99924879773009 0.00584644412981689 1.44487707435293e+28;0.0350734193867621 -0.00450250585817933 0.99937459578154 -2.4852029883325e+29;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.269473e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.441568e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.785395759505415;-9.97352133270435;-6.64927691046041]
Robot Arm 1 end: [0.995615618903248 -0.0103142778248196 0.092968570328123 -3.5790552567119e+29;0.00981279245616457 0.999934743112385 0.0058496684505095 -5.25541897192644e+27;-0.093022838594159 -0.00491173998915661 0.995651859994327 -3.03050728793983e+29;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97432381511629;6.41431302822787]
Robot Arm 2 end: [0.998698162200881 0.0372400547744346 -0.0348591327628747 7.15865683857891e+29;-0.0370419198672858 0.999293773199796 0.00631276616541532 -2.77569165592458e+28;0.035069602066864 -0.0050132987653587 0.999372297918229 -4.89226896040617e+28;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.262930e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.598981e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.785395759505415;-9.97216631030687;-6.65642921072278]
Robot Arm 1 end: [0.995604894632632 -0.0112859810841139 0.0929705351953707 3.0594322684265e+29;0.0108208557148579 0.999926296377699 0.00550553303285905 6.42541215185886e+27;-0.0930258182718265 -0.00447531488799389 0.995653638918429 7.30798864077071e+29;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97239630650286;6.41119449328468]
Robot Arm 2 end: [0.998643014897749 0.0386734260328926 -0.0348782871542334 -1.05923602234374e+30;-0.0384960453789619 0.99924224775383 0.00574323898637169 4.46892949748529e+28;0.0350739687819285 -0.00439276937159869 0.99937506487361 6.54884831891787e+29;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.652938e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.288333e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.785395759505415;-9.97274474149442;-6.65326468773523]
Robot Arm 1 end: [0.995609497572068 -0.0108829021871982 0.0929693002247471 1.57270958670479e+29;0.0103998120939778 0.999929793127263 0.00567914825285174 -2.89304529881539e+27;-0.0930245787558602 -0.00468735068581302 0.99565277908056 -1.2881422282516e+30;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97400821059608;6.41388161042412]
Robot Arm 2 end: [0.998688866758501 0.0374868097454325 -0.0348609596501811 7.78338192604e+29;-0.0372909923284548 0.999284899877601 0.00625066138675772 -3.734985185755e+28;0.0350703479278567 -0.0049424661579547 0.999372624562278 -1.82593288320867e+30;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.466839e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.474293e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.785395759505415;-9.97315722181556;-6.65135168758493]
Robot Arm 1 end: [0.995612764715522 -0.0105843430467809 0.0929687819527747 -1.29267718533128e+30;0.0100906488613294 0.999932389813353 0.0057788067723981 -9.01820411743487e+27;-0.0930236611893547 -0.00481533845367311 0.995652254039785 1.62347065647811e+30;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97331063036614;6.41253731225128]
Robot Arm 2 end: [0.998668599322505 0.0380175345196457 -0.03486682945543 5.08124001099569e+29;-0.0378283685026041 0.999265819252951 0.00606934997833557 -7.46384495243847e+27;0.0350719726228463 -0.004742313968505 0.99937353736957 3.25223360386046e+30;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.163801e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.879725e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.785395759505415;-9.97181525513105;-6.6580945658743]
Robot Arm 1 end: [0.99560211206105 -0.0115204304868526 0.0929715770597368 3.20411119157299e+30;0.0110679120588224 0.999924267820289 0.00538144466829461 3.51645348710706e+28;-0.0930265326787748 -0.00432877643882659 0.995654220054486 -1.56648330055673e+30;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97294455631157;6.41216063700693]
Robot Arm 2 end: [0.998658192292335 0.0382862190429244 -0.0348709104957605 -3.51322570603036e+30;-0.0381013643144524 0.999256161918692 0.00595053820270885 1.22733879249583e+29;0.0350727957936561 -0.00461392445990807 0.999374109478675 -4.31189983430829e+30;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.520847e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.601088e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.785395759505415;-9.9736265338837;-6.64883334072206]
Robot Arm 1 end: [0.995616436259741 -0.0102352482195062 0.0929685513673156 -5.48914677196601e+30;0.00973178992766266 0.999935432518181 0.00586711679457979 -6.52333166216258e+28;-0.0930226000187908 -0.00493664750235338 0.995651759099125 -7.39152765244809e+29;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97422098091218;6.4140570386623]
Robot Arm 2 end: [0.998695127612873 0.0373208441511289 -0.034859671171056 8.45688628103923e+30;-0.0371234163357401 0.999290867877107 0.00629391269250485 -3.17570842770134e+29;0.0350698451931328 -0.0049915898534153 0.99937239805233 2.21213479653441e+30;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.389732e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.700117e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.785395759505415;-9.97173978955795;-6.65860418870064]
Robot Arm 1 end: [0.995601516745181 -0.0115697318023047 0.0929718299309906 8.03519830280549e+30;0.0111200867817594 0.999923841791639 0.00535296988744922 1.20169705947495e+29;-0.0930266817889386 -0.00429557012194413 0.995654349939108 5.16720273396887e+30;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97233009821094;6.41107817015774]
Robot Arm 2 end: [0.998641220804841 0.0387188327171724 -0.0348792789839052 -1.48321873364455e+31;-0.0385424349816857 0.999240616350701 0.00571588494926262 6.05424332048235e+29;0.0350741046229266 -0.00436378598126639 0.999375187082814 3.38494083749767e+30;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.594046e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.850787e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.785395759505415;-9.97326404150739;-6.65064453428679]
Robot Arm 1 end: [0.995613605645048 -0.0105057023265146 0.0929686961998177 -6.46229159288319e+30;0.0100095684753138 0.999933074653185 0.00580127171973589 -1.27364753208128e+29;-0.0930234206713845 -0.00484524852354017 0.995652131406015 -1.3735333956597e+31;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97420315530022;6.41412927511193]
Robot Arm 2 end: [0.99869460210261 0.0373348116430107 -0.0348597700894703 1.57325041213887e+31;-0.0371375111305136 0.99929036559541 0.00629050834379061 -6.78622847780682e+29;0.0350698873414329 -0.00498769162772026 0.999372416036227 -1.80669309739862e+31;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.530664e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.603319e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.785395759505415;-9.97260619774154;-6.65425310013566]
Robot Arm 1 end: [0.995608395621753 -0.0109812062401124 0.0929695416627948 -1.68057962381177e+30;0.0105020940964404 0.999928939336406 0.00564112552244756 7.17947629781954e+28;-0.0930248815482586 -0.00463997705625933 0.995652972689707 2.49992347806459e+31;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97298195417948;6.41197655885181]
Robot Arm 2 end: [0.998659246461395 0.038259120686886 -0.0348704623058643 -7.42072410156148e+30;-0.0380738001924131 0.999257135836381 0.00596340665905554 4.43437814694497e+29;0.0350727129841222 -0.00462776018602472 0.999374048412102 3.97398132707982e+31;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.299823e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.563198e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.785395759505415;-9.97229714486576;-6.65563636484882]
Robot Arm 1 end: [0.995605935314145 -0.0111965542753317 0.092970203504162 2.1949045495372e+31;0.0107270556906154 0.999927071209598 0.00554820136675993 1.41322666476629e+29;-0.0930255440374104 -0.00452652566050725 0.99565343303882 -3.58583268155207e+31;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97327114650638;6.41264077288549]
Robot Arm 2 end: [0.998667467924595 0.0380468699581744 -0.0348672367052946 -2.42986654617388e+31;-0.0378581421910689 0.999264764765524 0.00605730698766458 7.15141323901413e+29;0.0350720626555965 -0.00472922662681424 0.999373596227454 -6.26241512951135e+31;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.929383e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.292114e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.785395759505415;-9.9734574112126;-6.64982219978966]
Robot Arm 1 end: [0.995615120519853 -0.0103620883741089 0.092968591022813 -5.95288694586416e+31;0.00986186296184965 0.999934326206029 0.00583840164907449 -5.7574550412221e+29;-0.0930229834565718 -0.00489595745706338 0.995651924193097 2.85007840076104e+31;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97403531125393;6.41367096842259]
Robot Arm 2 end: [0.998689662663801 0.0374657648310813 -0.0348607824652007 8.90626286583788e+31;-0.0372697312984806 0.999285656606591 0.00625648697538075 -3.21742912696676e+30;0.035070283965246 -0.00494903687157065 0.999372594289358 5.89321021886776e+31;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.329918e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.496566e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164057362947;-6.65909276043566]
Robot Arm 1 end: [0.995600736111726 -0.0116339324660016 0.0929721779306358 1.14343931088435e+32;0.0111881516512215 0.999923287314398 0.00531457891070276 1.45636244532417e+30;-0.093026875237313 -0.00425101184958722 0.995654523106201 -1.92582595559252e+30;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97238240971616;6.41114574172793]
Robot Arm 2 end: [0.998642637576655 0.0386829820683599 -0.0348784935489117 -1.84455983001533e+32;-0.0385058063131469 0.999241904412888 0.00573753827699059 7.30368030389222e+30;0.0350739974071525 -0.00438672584104215 0.999375090415145 -1.85590494964116e+31;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.912020e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.407998e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357137402843;-6.6491377812373]
Robot Arm 1 end: [0.995616008158023 -0.01027673790447 0.0929685589730496 -1.50314655465137e+32;0.00977429842229369 0.999935070545862 0.00585813816793813 -2.08150214571453e+30;-0.0930227251258235 -0.00492375369870752 0.995651811257168 -9.57816949211001e+31;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97431613899325;6.41423622455397]
Robot Arm 2 end: [0.9986979355187 0.0372460975683026 -0.0348591710546762 2.48710134011323e+32;-0.0370480138325826 0.999293555886308 0.00631140516519857 -1.01659481952922e+31;0.0350696202110525 -0.00501172425727987 0.999372305178717 -1.48718460323958e+32;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.610568e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.117389e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.785395759505415;-9.97204218700546;-6.65716329488246]
Robot Arm 1 end: [0.995603908771225 -0.0113698123512065 0.0929708782714102 1.56204486202892e+32;0.0109090002004184 0.999925570554124 0.0054632468950986 2.87841714524559e+30;-0.0930260745924834 -0.00442501063364653 0.995653839809201 2.59611971798029e+32;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97268850244158;6.41148718358505]
Robot Arm 2 end: [0.998651036310549 0.0384694688373734 -0.0348741687103345 -2.47949606663326e+32;-0.0382879554917481 0.999249576392617 0.00585803238622727 1.14469972503495e+31;0.0350733537051767 -0.00451486949385362 0.999374542308 4.29793524737867e+32;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.904992e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.161579e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.785395759505415;-9.97288189902315;-6.65266770272952]
Robot Arm 1 end: [0.995610586754912 -0.0107845710774516 0.0929690946939607 -6.26066490042969e+30;0.0102977478311704 0.999930647758751 0.00571454830854962 -1.91581430893854e+30;-0.0930242760312859 -0.00473209250126377 0.995652595722732 -4.92401858369542e+32;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97359813202472;6.41311621044525]
Robot Arm 2 end: [0.998676893948655 0.0378015294572298 -0.0348641056641541 -4.67041863620664e+31;-0.0376093702495662 0.999273584337853 0.00615133432772182 -1.32056928784768e+30;0.0350713096775436 -0.00483197840170579 0.999373131128823 -8.08603193743144e+32;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.762917e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.323172e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.785395759505415;-9.97303750116458;-6.65208664917367]
Robot Arm 1 end: [0.995611819308994 -0.0106718592468121 0.0929689016416331 -4.00038992253988e+32;0.0101810500459319 0.999931628100399 0.00575198613057007 -2.14828012962507e+30;-0.0930239295675991 -0.00478022433576818 0.995652398170718 6.84858029794205e+32;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97377747974421;6.413179612516]
Robot Arm 2 end: [0.998682112466123 0.0376647818112688 -0.0348626225535531 7.94626657268437e+32;-0.0374709335140637 0.999278500754418 0.00619734391387861 -2.68716418155629e+31;0.0350708908040072 -0.00488284149976128 0.999372898610475 1.03885243843378e+33;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.303745e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.916071e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.785395759505415;-9.97190943623266;-6.657710652033]
Robot Arm 1 end: [0.995602856656192 -0.0114583458876836 0.0929712758197332 1.12679209613079e+33;0.011002321872214 0.999924804591395 0.00541609419055178 1.15587413859598e+31;-0.0930263442872551 -0.00436937894659326 0.995654060302156 -7.56060692813076e+32;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9725464644033;6.41138402363601]
Robot Arm 2 end: [0.99864711660958 0.0385693258408537 -0.034876117782131 -2.05662780276285e+33;-0.0383897793488638 0.999245988162384 0.00580344578041612 7.85735335793758e+31;0.0350736557677777 -0.00445670792881161 0.999374792770721 -9.2002769564783e+32;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.865422e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.113408e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361089110066;-6.64903908603044]
Robot Arm 1 end: [0.995616314960049 -0.010247025814349 0.092968553002254 -2.1093771246016e+33;0.00974385293801908 0.999935329756474 0.00586460865985203 -2.39763772971425e+31;-0.0930226354996197 -0.00493302815429027 0.995651773723089 7.73619598015379e+30;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9743422417531;6.41419462398766]
Robot Arm 2 end: [0.998698706454095 0.0372255411149829 -0.034859042092594 3.42964554956701e+33;-0.0370272844318567 0.999294295148997 0.00631600271312193 -1.35046527801857e+32;0.0350695585161676 -0.00501704807297088 0.999372280631254 -7.69217999195828e+32;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.037726e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.371295e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.785395759505415;-9.97168220530577;-6.65899192181497]
Robot Arm 1 end: [0.995601063370979 -0.0116070796136674 0.0929720297585136 3.31694631788494e+33;0.0111596654413281 0.999923519190382 0.00533081955785933 4.75779487678876e+31;-0.0930267944294202 -0.00426983287293734 0.995654450120835 1.51866232695619e+33;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97246734737737;6.41112224445014]
Robot Arm 2 end: [0.998644949886937 0.0386243668257655 -0.0348772469186761 -4.60454971355562e+33;-0.0384459504844885 0.999244010454585 0.0057720414050638 1.98197724380533e+32;0.0350738215291954 -0.00442333109163456 0.999374935239719 3.97188794743712e+33;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.108093e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.325112e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.785395759505415;-9.97336025543699;-6.65026260462479]
Robot Arm 1 end: [0.995614360677146 -0.0104344412138583 0.0929686358402189 -3.12976992969739e+33;0.00993621906173081 0.999933695529529 0.00582031746701949 -5.46264802050561e+31;-0.0930232033645047 -0.00487103492228595 0.995652025887856 -4.83826735795385e+33;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97388997478092;6.41352991729148]
Robot Arm 2 end: [0.998685400413745 0.0375782847496977 -0.0348617772884261 2.45756985830532e+33;-0.0373834475826114 0.99928161074159 0.00622416906350733 -1.32544713003684e+32;0.0350706265594917 -0.00491273334953312 0.999372761387742 -9.44664977543718e+33;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602838e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.124177e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.785395759505415;-9.97246427250723;-6.65508795187434]
Robot Arm 1 end: [0.99560726580389 -0.0110807954382471 0.0929698244107066 7.5294136443952e+32;0.0106059720902821 0.999928075021322 0.00559983394153231 4.41496744108539e+31;-0.0930251881724625 -0.00458919999654642 0.995653179379682 9.5550657823696e+33;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97346757468619;6.41262131099401]
Robot Arm 2 end: [0.998673115562555 0.0379001295328889 -0.0348652898162293 4.98586541626538e+33;-0.0377092862437874 0.999270039671804 0.00611535324377912 -1.36265010334326e+32;0.0350716122179109 -0.00479249368307767 0.999373310640591 1.52529090769619e+34;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.927277e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.381546e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.785395759505415;-9.97243617299896;-6.65502703916857]
Robot Arm 1 end: [0.99560704207407 -0.0111003747619417 0.0929698846544706 6.43194657462418e+33;0.010626426012311 0.999927905173599 0.0055913795563617 2.54519305172305e+31;-0.0930252484152879 -0.00457887926056888 0.995653221268324 -1.46130951874015e+34;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97280106701414;6.41175892437781]
Robot Arm 2 end: [0.998654168508004 0.0383894101921901 -0.0348726957185975 -2.02107441209147e+34;-0.0382063953305209 0.999252453387606 0.00589964021766053 7.33065323007681e+32;0.03507311046135 -0.00455934029719846 0.999374348949692 -1.83638217809455e+34;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183485e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.601163e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.785395759505415;-9.97337781149199;-6.65035746010124]
Robot Arm 1 end: [0.995614498180209 -0.0104213958653473 0.092968626544591 -2.09653407778529e+34;0.00992280386338366 0.999933809220321 0.00582367080346862 -1.89173561524076e+32;-0.0930231636575468 -0.00487562163791201 0.99565200714747 1.39629568577009e+34;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97428053164321;6.41400578351799]
Robot Arm 2 end: [0.998696884336811 0.0372741028918792 -0.0348593526840477 4.25396579714229e+34;-0.0370762599252475 0.999292548756367 0.00630499404904853 -1.62228050869461e+33;0.0350697043885561 -0.00500432349160727 0.999372339311275 4.32816980919417e+33;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.334820e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.523960e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.785395759505415;-9.97167318253503;-6.65902055845423]
Robot Arm 1 end: [0.995600992406677 -0.0116129100231293 0.0929720616079595 4.30564730067372e+34;0.0111658483873961 0.999923468839264 0.00532731585731818 5.26771690000139e+32;-0.0930268119878844 -0.00426576901023168 0.995654465899753 -6.73927636541285e+33;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97234694513536;6.4109204160009]
Robot Arm 2 end: [0.998641676455776 0.0387073079758125 -0.0348790245686891 -6.90593926740344e+34;-0.038530658521265 0.99924103041394 0.00572289185686562 2.86087283587828e+33;0.035074070187466 -0.00437120653290442 0.999375155861867 2.75980029274212e+34;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.295728e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023482e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360589781821;-6.64908033053431]
Robot Arm 1 end: [0.995616276222589 -0.0102507833910833 0.0929685536110706 -6.15545192966429e+34;0.00974770220647408 0.999935296972473 0.00586380169048906 -8.09265766834896e+32;-0.0930226468251648 -0.0049318666284251 0.995651778419142 -2.83549139198855e+34;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97412029853385;6.41383820694094]
Robot Arm 2 end: [0.998692161665824 0.0373995867436109 -0.0348602515574737 6.3267184523178e+34;-0.0372028962666679 0.999288036283559 0.00627415730753041 -2.83781467716749e+33;0.0350700832136848 -0.00496904940156633 0.999372502028863 -9.92767135608513e+34;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.189726e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.749380e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.785395759505415;-9.97193009921012;-6.65783358926821]
Robot Arm 1 end: [0.995603020236131 -0.0114446433749493 0.0929712118599818 7.34016532602587e+34;0.0109878622640244 0.99992492310151 0.00542356370893076 1.24036053581154e+33;-0.0930263026222167 -0.00437816153861763 0.99565402561431 8.96138255765137e+34;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97313609875432;6.4120474487745]
Robot Arm 2 end: [0.99866361384055 0.0381465706559868 -0.0348686899398378 -4.59609249037654e+33;-0.0379593909718699 0.999261181088676 0.00601469918691797 9.31732908361856e+32;0.0350723684398054 -0.00468306699206811 0.99937380286636 1.97013824007872e+35;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.785655e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.474720e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.785395759505415;-9.97301250440459;-6.65210527922986]
Robot Arm 1 end: [0.995611621571025 -0.0106900461302755 0.0929689297907952 -2.73448794479277e+34;0.0101998592963754 0.999931469863837 0.00574616796493355 -1.01858739920155e+33;-0.0930239854179952 -0.00477268160258626 0.995652429137434 -1.84841913763951e+35;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97311361332895;6.41221944445233]
Robot Arm 2 end: [0.998662974589548 0.0381630730053631 -0.0348689409466756 -1.63707010829423e+35;-0.0379761584258543 0.99926058794513 0.00600739300929303 5.36492686388338e+33;0.0350724190093845 -0.00467517254665633 0.999373838053853 -2.86696603199283e+35;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.383496e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.822430e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.785395759505415;-9.97290852334082;-6.65286608318297]
Robot Arm 1 end: [0.995610797913218 -0.0107653710260973 0.0929690586446687 -1.13649126179644e+35;0.0102778469126698 0.999930814702516 0.0057211599525828 -3.01088632606575e+32;-0.0930242169622788 -0.00474052687301518 0.99565256111935 2.81118987363464e+35;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97413333561549;6.41367914671149]
Robot Arm 2 end: [0.998692545401156 0.0373894111772102 -0.0348601734358983 4.7467795749753e+35;-0.0371926227191527 0.999288402191301 0.00627678749260459 -1.74942876034986e+34;0.0350700524013035 -0.00497203959940583 0.99937248823789 2.16472928140288e+35;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.188096e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.066732e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.785395759505415;-9.97201883463928;-6.65724994280447]
Robot Arm 1 end: [0.995603723501458 -0.0113854717358993 0.0929709459099115 3.85642573811643e+35;0.0109254888099628 0.999925435030796 0.00545509603364569 3.62118316477395e+33;-0.0930261223759006 -0.00441536089396529 0.995653878184522 -3.42845378025515e+35;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97234287567749;6.41090094296582]
Robot Arm 2 end: [0.998641566336749 0.0387100936644542 -0.0348790858964808 -9.0606095632433e+35;-0.0385335049132607 0.999240930328795 0.00572120221134108 3.6405294695482e+34;0.0350740785136916 -0.00436941690990084 0.99937516339575 4.83581679426671e+34;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.891573e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023831e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357952620442;-6.64932668557674]
Robot Arm 1 end: [0.995616071492925 -0.0102706132576521 0.0929685575251444 -7.95683023646412e+35;0.00976802102951454 0.999935123973897 0.00585948875527544 -8.63968858387415e+33;-0.0930227066374728 -0.00492568235049506 0.99565180344497 1.13686625720887e+35;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97427314791019;6.41401362129291]
Robot Arm 2 end: [0.998696666428691 0.0372799049261096 -0.0348593911725319 1.09788294984889e+36;-0.0370821125863596 0.999292340105004 0.00630364446945759 -4.5891576159444e+34;0.0350697218459445 -0.00500276884984717 0.999372346482273 -9.04302639984174e+35;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.292111e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.430206e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164538018495;-6.65927327461877]
Robot Arm 1 end: [0.995600773872137 -0.0116308385982001 0.0929721606649265 1.33501050586597e+36;0.0111848683286729 0.999923314027012 0.00531646364681489 1.84346677638736e+34;-0.0930268659349145 -0.00425319394576257 0.995654514656357 3.95713438748066e+35;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9728209943383;6.41151696070338]
Robot Arm 2 end: [0.998654725280938 0.0383751546181938 -0.0348724415847769 -8.58513708242861e+35;-0.0381918792540704 0.999252965694805 0.00590685273039215 4.24976558462589e+34;0.0350730670614428 -0.00456706231244432 0.999374315213643 2.2623523523077e+36;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.155157e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.029177e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.785395759505415;-9.97344272661733;-6.6499528635802]
Robot Arm 1 end: [0.99561500585005 -0.010373048854436 0.09296859676078 -1.4242160381582e+36;0.00987311940562676 0.999934230648555 0.00583574249641605 -2.27255926944431e+34;-0.0930230167184835 -0.00489226274291591 0.995651939246767 -1.66110173038213e+36;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97344523668611;6.41270476655697]
Robot Arm 2 end: [0.998672470978842 0.0379169161913254 -0.034865501223323 -8.14715023780999e+35;-0.0377263052928451 0.999269436213952 0.0061089882633626 1.68499304574208e+34;0.0350716637467425 -0.00478553186081393 0.999373342193419 -3.89915143973643e+36;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.054811e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.382093e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.785395759505415;-9.97232449675671;-6.65590843600349]
Robot Arm 1 end: [0.995606153006349 -0.0111777246627709 0.0929701380404519 8.15868263350458e+35;0.0107073344689703 0.999927234429311 0.00555687278240003 2.30141844938941e+34;-0.093025486209248 -0.00453699437000553 0.995653390793011 3.5745296692675e+36;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97390790975153;6.4132338840254]
Robot Arm 2 end: [0.998685925550462 0.0375644454865013 -0.034861648594237 4.63046959449537e+36;-0.0373694557227765 0.999282108344336 0.00622830007972476 -1.62557165627199e+35;0.035070584346427 -0.0049173547961606 0.999372740140238 4.39236503707268e+36;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.503166e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.206731e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.785395759505415;-9.97257794504912;-6.6544045168288]
Robot Arm 1 end: [0.995608170762228 -0.0110011227124902 0.0929695950865409 1.67627592768751e+36;0.0105228471498083 0.999928766433239 0.00563309392454945 -1.07727204107686e+33;-0.0930249428881974 -0.00463004949927726 0.995653013173908 -5.82475489108286e+36;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9724556682599;6.41106118300549]
Robot Arm 2 end: [0.998644631079825 0.0386324559538596 -0.0348774162232834 -1.06442096091367e+37;-0.0384542085249834 0.999243719806897 0.00576734541986468 4.14116584711538e+35;0.0350738458421107 -0.00441834510286791 0.999374956442474 -2.93430618148206e+36;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.695488e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.364112e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.785395759505415;-9.97328430658747;-6.65096279404567]
Robot Arm 1 end: [0.995613764872652 -0.0104907260724926 0.0929686821600437 -7.17074596191778e+36;0.00999414375505443 0.999933205113102 0.00580537706293097 -5.90533165189148e+34;-0.0930233749479484 -0.0048507709399046 0.995652108788297 6.41015581436279e+36;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.9743408240645;6.4140429057414]
Robot Arm 2 end: [0.998698664576021 0.0372266581427873 -0.0348590490058353 1.60492170592957e+37;-0.0370284107754849 0.999294254977607 0.00631575523522523 -6.42522321477011e+35;0.0350695618665694 -0.00501676113337762 0.999372281954136 -6.26558774697198e+36;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.444226e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.921379e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.785395759505415;-9.97172675634797;-6.65884206688231]
Robot Arm 1 end: [0.995601414061865 -0.0115782055754663 0.0929718746378035 1.58921416643045e+37;0.0111290625761463 0.999923768587084 0.00534798847973146 1.85855917927507e+35;-0.0930267073704719 -0.00428977508214146 0.995654372533739 -4.7925892818249e+36;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97256130904019;6.41108707767865]
Robot Arm 2 end: [0.998647524497556 0.0385589517457069 -0.0348759093813306 -1.91203508233245e+37;-0.038379195951613 0.999246360929164 0.00580925881073382 8.44971490783237e+35;0.0350736244635512 -0.00446289257036816 0.999374766269839 2.26989204662923e+37;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446382e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.621762e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.785395759505415;-9.9736247208115;-6.64910546061936]
Robot Arm 1 end: [0.995616422204883 -0.0102366137618011 0.0929685515356676 -2.46633017740002e+37;0.00973318840853082 0.999935420603192 0.00586682763689475 -3.09164705544725e+35;-0.0930226041312132 -0.00493622951337302 0.995651760787294 -7.5089767461095e+36;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97375769354928;6.41315418343449]
Robot Arm 2 end: [0.99868153527504 0.0376799390979912 -0.0348627780054922 5.08957937287969e+36;-0.0374862703728748 0.999277955792841 0.00619246316192854 -3.84917890085388e+35;0.0350709371733959 -0.00487742309542553 0.999372923442359 -4.77388664991916e+37;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.149030e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.659517e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.785395759505415;-9.97183275190953;-6.65842675468967]
Robot Arm 1 end: [0.995602250259634 -0.011508942917456 0.092971519891178 3.26758623233523e+37;0.0110557664543637 0.999924367116397 0.00538795647519644 5.18182173833545e+35;-0.0930264978705505 -0.00433639018018245 0.995654190175558 2.98727365496495e+37;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97361955558205;6.41270170879193]
Robot Arm 2 end: [0.998677515675256 0.0377852720517812 -0.0348639197548427 3.58813188657922e+37;-0.0375929038376248 0.999274168810864 0.00615703893215547 -1.13062403736216e+36;0.035071259825594 -0.00483826036193439 0.999373102485211 6.95338952450343e+37;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.534758e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.799759e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.785395759505415;-9.97313423901377;-6.65158944240027]
Robot Arm 1 end: [0.995612583450519 -0.0106011954618801 0.0929688029919864 -1.96645308964862e+37;0.0101080427850444 0.999932243100768 0.00577379238654256 -4.88236573967868e+35;-0.0930237128158164 -0.00480872771595521 0.995652281166329 -6.81204561608396e+37;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97267072580587;6.4113766455198]
Robot Arm 2 end: [0.998650543711206 0.0384820383072545 -0.0348744071352026 -1.11533147851687e+38;-0.0383007667284357 0.999249124710372 0.0058513275048813 4.15944432645854e+36;0.0350733918138356 -0.00450771486170415 0.9993745732674 -7.4261665809631e+37;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.365706e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.851078e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.785395759505415;-9.97277254049365;-6.65367811463745]
Robot Arm 1 end: [0.995609718492944 -0.0108630522122484 0.0929692558808297 -2.74668794155646e+37;0.0103791889456106 0.999929965602432 0.00568650394797349 8.919840742407e+34;-0.0930245176242937 -0.00469659312193094 0.995652741237533 1.1265445313095e+38;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97432073985123;6.41392332327205]
Robot Arm 2 end: [0.998698071382822 0.0372424759150882 -0.0348591480668558 2.0901314209353e+38;-0.0370443614773912 0.999293686129635 0.00631222180995351 -8.0913491598194e+36;0.0350696093357949 -0.00501266886595864 0.999372300822809 -1.08915739529672e+37;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.832783e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.931092e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.785395759505415;-9.97214079697954;-6.65672595884879]
Robot Arm 1 end: [0.995604691854872 -0.0113032937557757 0.0929706034552627 1.27663719795505e+38;0.0108390419539987 0.999926146439677 0.00549698424751462 1.05767696264312e+36;-0.0930258712729128 -0.00446511103653771 0.995653679778941 -1.48890564551237e+38;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97239025453985;6.41080411389645]
Robot Arm 2 end: [0.998642850528205 0.0386775892766519 -0.0348783769210677 -3.12187617298149e+38;-0.0385002977624996 0.999242098171384 0.00574075903068719 1.31464376982353e+37;0.035073981255345 -0.00439014006566719 0.999375075989543 1.85900448925584e+38;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.297545e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.890748e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.785395759505415;-9.97353261239252;-6.64969476397232]
Robot Arm 1 end: [0.995615706715277 -0.0103058244624003 0.0929685673977498 -2.94406005532068e+38;0.00980412155414811 0.999934816838305 0.00585160448901833 -3.0435200583588e+36;-0.0930228130212758 -0.00491447420326972 0.995651848891426 8.49667567305711e+37;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97401983713941;6.4135169461922]
Robot Arm 2 end: [0.998689208152275 0.0374777846851852 -0.0348608831682735 2.35266091258181e+38;-0.0372818741596561 0.999285224397889 0.00625317193139036 -1.11967583919134e+37;0.035070320490761 -0.00494529626522623 0.999372611524613 -5.28738822118578e+38;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.290390e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.711416e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163020081765;-6.65944409966805]
Robot Arm 1 end: [0.99560065464366 -0.0116406034796012 0.0929722153320345 5.2540637006961e+38;0.0111952322970383 0.999923229719301 0.00531050294566062 6.99412701749089e+36;-0.093026895288034 -0.00424629466137441 0.995654541361972 7.82746009313208e+37;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97329393528065;6.41212817171831]
Robot Arm 2 end: [0.998668120685524 0.0380299485943018 -0.0348670006791313 1.32094071740205e+38;-0.0378409670880819 0.999265373015999 0.00606428075166606 -1.53745131151423e+36;0.0350720107248285 -0.0047368028364166 0.999373562169125 9.51473878712396e+38;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.545690e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.282810e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.785395759505415;-9.97351064518895;-6.64971964512201]
Robot Arm 1 end: [0.995615535663742 -0.0103222828290856 0.0929685733040827 -6.20903967252363e+38;0.0098210049074024 0.999934673300376 0.00584781961747306 -9.22416358063402e+36;-0.0930228628220451 -0.00490913524626178 0.995651870577023 -5.52531852759139e+38;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97296035674736;6.41180368140053]
Robot Arm 2 end: [0.998658637407251 0.0382747802021863 -0.0348707202731314 -1.00447189679942e+39;-0.0380897279925262 0.999256573029571 0.00595599518567611 3.49221722163367e+37;0.0350727608458191 -0.0046197897864244 0.999374083608827 -1.27333787005143e+39;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.196894e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.889190e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.785395759505415;-9.97219006197891;-6.65669848244863]
Robot Arm 1 end: [0.995605083462581 -0.0112698265122299 0.0929704726988751 5.07095318356795e+38;0.0108038939688789 0.999926436305488 0.00551342475378081 1.09946454506151e+37;-0.0930257687878862 -0.00448475058287741 0.995653601085052 1.30751382580314e+39;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97421360702004;6.41366040156342]
Robot Arm 2 end: [0.99869491020626 0.0373266234079828 -0.0348597118902066 2.45621605334786e+39;-0.0371292480844185 0.999290660050143 0.00629250929562502 -9.20791806325251e+37;0.0350698626286913 -0.00498998211504345 0.999372405469401 6.85679226147494e+38;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.160832e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.002885e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.785395759505415;-9.97271932060283;-6.6537848733473]
Robot Arm 1 end: [0.995609295487591 -0.0109010179215332 0.0929693419734547 3.3456455155563e+38;0.0104186421944523 0.999929635742335 0.00567234158217389 -4.4124185370207e+36;-0.0930246345519655 -0.00467882169731801 0.99565281398387 -2.27593174533741e+39;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97232992059629;6.41069699386239]
Robot Arm 2 end: [0.998641216004142 0.038718954114267 -0.0348792816735774 -4.34258109548588e+39;-0.0385425590384667 0.999240611989143 0.00571581090744798 1.76988154712106e+38;0.0350741049854985 -0.00436370758194043 0.999375187412418 9.1638006348705e+38;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.374476e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.416571e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.785395759505415;-9.97317796542587;-6.65163208489244]
Robot Arm 1 end: [0.99561292822207 -0.0105691117663175 0.0929687637518473 -2.36291483855304e+39;0.0100749338710315 0.999932522426837 0.00578327766903296 -1.70227746640837e+37;-0.093023614553349 -0.0048212518679158 0.995652229779985 2.81349450239303e+39;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97421086896321;6.41375790069198]
Robot Arm 2 end: [0.998694829486355 0.0373287688652695 -0.0348597270818951 4.66987602692482e+39;-0.0371314130936973 0.999290582897851 0.0062919864381496 -2.00746529452643e+38;0.03506986910278 -0.00498938339636688 0.999372408231505 -5.19502795230578e+39;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.701805e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.009095e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.785395759505415;-9.97179998546908;-6.6585640725155]
Robot Arm 1 end: [0.995601991505206 -0.0115304383848449 0.0929716274006255 5.74416786475571e+39;0.0110784967497034 0.999924181322806 0.00537573396684482 6.37225233894161e+37;-0.0930265629840974 -0.00432210557122042 0.995654246203368 -2.63314528173002e+39;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97296608950553;6.41156961595159]
Robot Arm 2 end: [0.998658799001914 0.0382706262717744 -0.0348706515686333 -2.36269954241643e+39;-0.0380855026635036 0.999256722322253 0.00595796782935271 1.37039710754734e+38;0.0350727481518502 -0.00462191070375813 0.999374074247738 1.15583690140397e+40;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.469701e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.230156e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362775878573;-6.64921346205388]
Robot Arm 1 end: [0.995616445754482 -0.0102343255977096 0.0929685512567133 -9.68873505607727e+39;0.00973084507388155 0.99993544056854 0.00586731191837089 -1.16080087776957e+38;-0.0930225972404565 -0.00493692966927905 0.995651757959619 -1.55220898966564e+39;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97328789180026;6.41228504345042]
Robot Arm 2 end: [0.998667947510924 0.0380344387518938 -0.0348670630129104 -6.79723544502871e+39;-0.0378455243245785 0.999265211613442 0.00606243744217338 1.96650321533927e+38;0.0350720245055167 -0.00473479967590696 0.999373571178022 -1.83678152802344e+40;0 0 0 1]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.362671e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.541247e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.785395759505415;-9.97175250225686;-6.65893124302833]
Robot Arm 1 end: [0.995601616941398 -0.0115614547907357 0.0929717866172097 1.39837385039584e+40;0.0111113217291812 0.9999239133021 0.00535781075112308 2.10863833928379e+38;-0.0930266567877445 -0.00430120561424354 0.995654327945778 9.51578421701073e+39;0 0 0 1]
Mobile Platform 2 Pose: [-2.35234255166918;9.97402400152083;6.4132683637229]
Robot Arm 2 end: [0.9986893304544 0.0374745508223406 -0.0348608559448345 2.56962797567797e+40;-0.0372786070905828 0.999285340680517 0.00625406702951246 -9.26090165212434e+38;0.0350703106619921 -0.00494630586271184 0.999372606873124 1.76235929608381e+40;0 0 0 1]
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.843261e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 2:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 end: [-9.8543202540212;-9.82617087977973;1.50722002620516]
Mobile Platform 2 Pose: [-2.35629185848574;9.99866771650191;9.81844390794743]
Robot Arm 2 end: [9.85353115927021;9.82440256999551;1.50725286605054]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.308015e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 end: [-9.72929347716611;-9.65232574912907;1.40200599186901]
Mobile Platform 2 Pose: [-2.35629174017327;9.99733167735659;9.63690091275154]
Robot Arm 2 end: [9.72764003092795;9.64801727430815;1.4056825549176]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.962713e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 end: [-9.69930061746444;-9.47951608220887;1.23897968990596]
Mobile Platform 2 Pose: [-2.35629162185557;9.99599115710093;9.45545425528116]
Robot Arm 2 end: [9.69154743885102;9.46812272306475;1.24887286397811]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.774943e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 end: [-9.77867309312711;-9.30834404725646;1.12386821017013]
Mobile Platform 2 Pose: [-2.35629150372541;9.994642733491;9.27450987506891]
Robot Arm 2 end: [9.75805533135666;9.28464215414943;1.13383266392774]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.720506e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 end: [-9.85305702661775;-9.13781278677307;1.12021872025027]
Mobile Platform 2 Pose: [-2.35629138595898;9.99328288711612;9.09451174108658]
Robot Arm 2 end: [9.82638677792646;9.10209881134701;1.11812539746746]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.781860e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346624]
Robot Arm 1 end: [-9.83163316704073;-8.96674749629691;1.10413295360329]
Mobile Platform 2 Pose: [-2.35629126836922;9.99191235800845;8.91540929190746]
Robot Arm 2 end: [9.81758164506882;8.92353965354058;1.09909329088906]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397341528709;-9.99002831581144;-8.79949883355178]
Robot Arm 1 end: [-9.88437209684222;-8.79686073268705;0.993019402149946]
Mobile Platform 2 Pose: [-2.3562911505223;9.9905342819858;8.73682082008527]
Robot Arm 2 end: [9.86446577937946;8.74321909246583;1.01042331278774]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274174]
Robot Arm 1 end: [-10.0240635535027;-8.62844670979544;1.06365451935043]
Mobile Platform 2 Pose: [-2.35610926515416;9.98914654622391;8.55900549404314]
Robot Arm 2 end: [9.96949458971284;8.56148620891408;1.05871774214693]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397111196129;-9.98711511781;-8.46062525935788]
Robot Arm 1 end: [-9.83059699093515;-8.4567607557185;1.11116984223786]
Mobile Platform 2 Pose: [-2.3559325329828;9.98774923574186;8.38210573741556]
Robot Arm 2 end: [9.84188326773804;8.38983262208401;1.07398930899778]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785396995929675;-9.98565931998689;-8.29072857539469]
Robot Arm 1 end: [-9.9514182120527;-8.28880184743121;0.775243010355493]
Mobile Platform 2 Pose: [-2.35574218607075;9.98635047820884;8.20503420298597]
Robot Arm 2 end: [9.97772919407588;8.2070334841914;0.847078801545182]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396877283108;-9.98418232823806;-8.12462828169963]
Robot Arm 1 end: [-10.2177197802531;-8.12505481884846;1.34880830987888]
Mobile Platform 2 Pose: [-2.35560327793646;9.98493698510815;8.03016767314834]
Robot Arm 2 end: [10.0962616130589;8.02818259337649;1.27733992745207]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840343624;-7.95994828886092]
Robot Arm 1 end: [-9.14378697899513;-7.94738998499866;0.731100312735129]
Mobile Platform 2 Pose: [-2.35467490025416;9.98351838765136;7.85622999887345]
Robot Arm 2 end: [9.39762314478083;7.88154034035604;0.734858785942836]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455233634;-7.79314279537029]
Robot Arm 1 end: [-11.319324102881;-7.8090058656453;0.799171001188725]
Mobile Platform 2 Pose: [-2.35418610721657;9.98211893885527;7.68192332961784]
Robot Arm 2 end: [10.9161463469199;7.64627256242753;1.00162401288452]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396515982167;-9.97972356094616;-7.63794018923431]
Robot Arm 1 end: [-7.79091628713719;-7.60341009704661;2.47325654548148]
Mobile Platform 2 Pose: [-2.35378927016398;9.98066881059322;7.51575630925178]
Robot Arm 2 end: [8.27552039648346;7.58918984210124;1.83218767001604]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591664651;-7.46636918684994]
Robot Arm 1 end: [-12.0397763896495;-7.50088735641489;-3.02355377029781]
Mobile Platform 2 Pose: [-2.35322284591672;9.97930446269059;7.33822203524682]
Robot Arm 2 end: [11.778635209041;7.26134227242754;-1.6205784117868]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396271346212;-9.9767881848376;-7.31817266015568]
Robot Arm 1 end: [-9.67949070656236;-7.29011847767718;9.12697151192072]
Mobile Platform 2 Pose: [-2.35236174639594;9.97782824790403;7.17880323047707]
Robot Arm 2 end: [8.99530119127063;7.23646678111975;6.68419278486522]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396152232725;-9.97535901195325;-7.16418511935008]
Robot Arm 1 end: [-3.71065304441577;-7.11653477563282;-12.0805986259663]
Mobile Platform 2 Pose: [-2.35110928012804;9.97645061081865;7.01005870885193]
Robot Arm 2 end: [6.73069841103017;7.11614835940505;-8.72707663873835]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.78539602676002;-9.97430990778758;-7.00084677780544]
Robot Arm 1 end: [-30.2146663511286;-7.21213986521875;16.5803737834558]
Mobile Platform 2 Pose: [-2.3509408356558;9.9752375531767;6.84195887566461]
Robot Arm 2 end: [22.4550261337576;6.37749695044128;14.6326440012171]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.78539595660333;-9.97259808477923;-6.91581468798799]
Robot Arm 1 end: [34.946243018942;-6.35580498868623;-7.65591218699667]
Mobile Platform 2 Pose: [-2.35078477559672;9.9736342324962;6.72931268163405]
Robot Arm 2 end: [-21.1701767467381;7.97078029952425;-14.5810209836752]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395855227853;-9.97409553093641;-6.77330520680139]
Robot Arm 1 end: [-81.3808996754801;-7.72739711753274;-24.1214617243549]
Mobile Platform 2 Pose: [-2.35014502119003;9.97448137401582;6.56763418776644]
Robot Arm 2 end: [70.6898168604828;4.13399984493559;4.90347651804618]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395822155724;-9.97228129699363;-6.73833367577707]
Robot Arm 1 end: [76.361968467638;-5.31119500547159;100.00122885523]
Mobile Platform 2 Pose: [-2.34778304578035;9.97259564888566;6.51358265835416]
Robot Arm 2 end: [-91.225072552401;10.7265507008082;40.6486631006703]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395795540074;-9.97333331315975;-6.69737167020613]
Robot Arm 1 end: [-34.8235964304036;-7.77939710335487;-213.071696954009]
Mobile Platform 2 Pose: [-2.34774920024393;9.97428676330583;6.47077710352631]
Robot Arm 2 end: [126.045091865645;1.39097751011757;-134.440944713463]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395781195851;-9.97294909645961;-6.68023583785295]
Robot Arm 1 end: [-168.168980752377;-7.38861950995623;325.748462689486]
Mobile Platform 2 Pose: [-2.34774918378372;9.97308980750159;6.44692184435612]
Robot Arm 2 end: [-48.907198316654;9.89467060032564;304.550391077952]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395772638942;-9.97200303863952;-6.6734799530773]
Robot Arm 1 end: [506.203134914948;-1.47056316889379;-352.803824392812]
Mobile Platform 2 Pose: [-2.34774917338603;9.97332796729133;6.43383336625037]
Robot Arm 2 end: [-169.353595857279;11.6843322366954;-497.273120331896]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395767401621;-9.97358488645272;-6.65848980977463]
Robot Arm 1 end: [-1005.55321699434;-18.1123333231205;26.7195979469092]
Mobile Platform 2 Pose: [-2.34774916733206;9.9739294646721;6.42664949967268]
Robot Arm 2 end: [714.736339995033;-19.1245840542175;526.547253686581]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395764311851;-9.97165007575082;-6.66429501649506]
Robot Arm 1 end: [1511.85265544493;15.229621163314;813.676929300762]
Mobile Platform 2 Pose: [-2.34774916382041;9.97235427760939;6.41958597753304]
Robot Arm 2 end: [-1501.03717657867;65.7836146636269;-185.101068348254]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785395762440954;-9.97334587567502;-6.6529973067721]
Robot Arm 1 end: [-1351.19762068305;-31.2082417937703;-2373.62082194952]
Mobile Platform 2 Pose: [-2.3450665028873;9.97427858861498;6.4198788915864]
Robot Arm 2 end: [2173.77480195722;-81.3260948561547;-1001.70509465282]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785395761295572;-9.97254300194253;-6.65580126996799]
Robot Arm 1 end: [46.0170625865686;10.6125492873528;4517.87359654708]
Mobile Platform 2 Pose: [-2.34205026446011;9.97262156548858;6.41535537515439]
Robot Arm 2 end: [-2415.33234519052;114.654253245537;3302.7170571722]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395760593854;-9.97237811571088;-6.65550572034403]
Robot Arm 1 end: [3450.44583680637;11.4998906929861;-6706.85912137023]
Mobile Platform 2 Pose: [-2.34205026368323;9.97370787965216;6.41623489125978]
Robot Arm 2 end: [300.195840396401;-30.7450904510039;-6638.35463004332]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395760172427;-9.97341831968126;-6.64975907458716]
Robot Arm 1 end: [-10301.8682886198;-102.703915782629;5930.57033264479]
Mobile Platform 2 Pose: [-2.34205026321295;9.97368517556955;6.41534201331612]
Robot Arm 2 end: [5595.95748780081;-174.460230044394;9232.32433957167]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395759909199;-9.9716570058922;-6.65837171279946]
Robot Arm 1 end: [20469.3647810461;248.680624951673;-1882.17757559135]
Mobile Platform 2 Pose: [-2.34205026292347;9.97262151536404;6.41344055857881]
Robot Arm 2 end: [-16030.4831809692;607.70196585229;-9257.66486094128]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395759753156;-9.97359339942042;-6.64819520423459]
Robot Arm 1 end: [-28186.7052666001;-385.77615216232;-15181.8065364059]
Mobile Platform 2 Pose: [-2.342050262752;9.97433259866007;6.41587149741306]
Robot Arm 2 end: [28720.2676465408;-1107.03261729776;-3373.19477928298]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785395759655636;-9.97198118932992;-6.65650177619343]
Robot Arm 1 end: [31681.8050003884;548.612483294391;44427.6506801174]
Mobile Platform 2 Pose: [-2.34205026264504;9.97241710316464;6.41261583978792]
Robot Arm 2 end: [-41241.9354543084;1749.72986330834;28475.6249640968]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395759597111;-9.9729537474989;-6.65125334956953]
Robot Arm 1 end: [-7502.89364253326;-425.746370587626;-88010.3488691388]
Mobile Platform 2 Pose: [-2.34205026258155;9.97397252293369;6.41513719347186]
Robot Arm 2 end: [27868.9936417286;-1364.08417111768;-75059.2566917786]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785395759561428;-9.97296922404905;-6.65134925669457]
Robot Arm 1 end: [-62264.5244342754;-262.75741195823;128556.420461561]
Mobile Platform 2 Pose: [-2.34205026254268;9.97336128453355;6.41387690662363]
Robot Arm 2 end: [27169.3591687903;-555.676547727233;129588.941937139]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785395759539367;-9.97196650329117;-6.6562907870714]
Robot Arm 1 end: [191892.300660036;1876.69459747752;-150202.207985042]
Mobile Platform 2 Pose: [-2.34205026251895;9.97289719293433;6.4133108478367]
Robot Arm 2 end: [-149793.23786854;5284.09746774724;-166869.277120609]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.78539575952626;-9.97359580922974;-6.64798611149885]
Robot Arm 1 end: [-378054.199328173;-4200.3551522971;29548.7276718451]
Mobile Platform 2 Pose: [-2.34205026250485;9.97424184956201;6.4153059693754]
Robot Arm 2 end: [344525.300634353;-12934.5771425875;72079.3406282409]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785395759518025;-9.9716595880824;-6.65794808095914]
Robot Arm 1 end: [615946.863632221;8646.93460906925;227091.850910338]
Mobile Platform 2 Pose: [-2.34205026249605;9.97233227801684;6.41227315281111]
Robot Arm 2 end: [-589640.197048739;24080.6645547508;165847.383933014]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785395759513141;-9.97340627582553;-6.64887127704]
Robot Arm 1 end: [-622984.033453167;-10337.6696672214;-825231.571272748]
Mobile Platform 2 Pose: [-2.34205026249085;9.9741782945121;6.41528278214184]
Robot Arm 2 end: [598885.854997012;-26006.0721908307;-758027.237502747]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785395759510113;-9.97238834368967;-6.65431341155916]
Robot Arm 1 end: [270262.987481037;9520.57093579113;1705517.42966449]
Mobile Platform 2 Pose: [-2.34205026248764;9.97303035928066;6.41323682929826]
Robot Arm 2 end: [-215540.058822873;14583.4867727513;1611768.05373209]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785395759508273;-9.9725122048813;-6.65347070990428]
Robot Arm 1 end: [961279.235835827;1820.69918996546;-2698709.18725963]
Mobile Platform 2 Pose: [-2.34205026248569;9.97322012944097;6.41374194695057]
Robot Arm 2 end: [-1099077.59636174;33593.0093407338;-2476205.93563805]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785395759507166;-9.97332888202583;-6.64945225469874]
Robot Arm 1 end: [-3570605.76458154;-30789.6230211075;2793563.7687929]
Mobile Platform 2 Pose: [-2.34205026248453;9.97406821786966;6.41491125603027]
Robot Arm 2 end: [3699439.38181385;-133896.377693459;2188697.54735849]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785395759506475;-9.97169930690876;-6.65771324271522]
Robot Arm 1 end: [7628562.99210222;91153.6004492751;-1782474.65460735]
Mobile Platform 2 Pose: [-2.34205026248381;9.97236655326828;6.41229677632595]
Robot Arm 2 end: [-7457375.50114528;295363.951452732;-382077.782394052]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785395759506066;-9.97361778207248;-6.64785910516261]
Robot Arm 1 end: [-11399858.1306472;-146024.25219677;-4269416.11561628]
Mobile Platform 2 Pose: [-2.34205026248338;9.97430413820145;6.41540191002785]
Robot Arm 2 end: [9725610.55732499;-398451.967948943;-6470324.04942666]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785395759505809;-9.97187540406896;-6.65693891215203]
Robot Arm 1 end: [14432101.4284901;235168.317966706;15064223.8848676]
Mobile Platform 2 Pose: [-2.34205026248312;9.97272896053198;6.4127268218712]
Robot Arm 2 end: [-9074614.79282536;424821.154625248;17780314.5010945]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785395759505656;-9.97307917309742;-6.65059553957304]
Robot Arm 1 end: [-7289792.3513148;-209245.314149012;-32717159.8281838]
Mobile Platform 2 Pose: [-2.34205026248296;9.97354953103527;6.4142184397412]
Robot Arm 2 end: [-3429655.21974407;7397.62587802717;-32492491.2971768]

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785395759505562;-9.97283566167774;-6.6520756892268]
Robot Arm 1 end: [-16454809.5334623;-2516.3429982175;52085162.7418711]
Mobile Platform 2 Pose: [-2.34205026248286;9.9738203002935;6.41443154100018]
Robot Arm 2 end: [34059842.8781837;-1161661.19706897;40117302.6657246]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036872e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782991e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785395759505505;-9.97208304302557;-6.65574885814607]
Robot Arm 1 end: [64570619.2862605;570222.364414744;-66454004.2931879]
Mobile Platform 2 Pose: [-2.3420502624828;9.97251465851256;6.41250951491704]
Robot Arm 2 end: [-84572834.2323376;3235808.71756953;-33015871.2671306]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.811171e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.141109e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.78539575950547;-9.97355589918601;-6.64829011506196]
Robot Arm 1 end: [-141162817.066184;-1493574.65463057;31103018.3387904]
Mobile Platform 2 Pose: [-2.34205026248277;9.97434358820683;6.41538222203493]
Robot Arm 2 end: [136524862.630339;-5379055.91675847;-37671202.9512269]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563931e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.132099e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785395759505448;-9.97163424245395;-6.6581539755762]
Robot Arm 1 end: [244914598.687377;3315570.49558182;52795429.2676467]
Mobile Platform 2 Pose: [-2.34205026248275;9.97249468392965;6.41233950991854]
Robot Arm 2 end: [-176949742.57028;7642336.00830289;169040719.734946]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056079e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.165419e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785395759505436;-9.97348102384585;-6.64859180489613]
Robot Arm 1 end: [-276643644.999971;-4239890.34380551;-278783299.46329]
Mobile Platform 2 Pose: [-2.34205026248274;9.97384870256385;6.41464451472206]
Robot Arm 2 end: [80947407.3830689;-4642934.65864805;-385355256.268852]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231848e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.659175e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785395759505428;-9.97225080607788;-6.65511445629158]
Robot Arm 1 end: [197163198.179182;4714815.24863253;630522254.33242]
Mobile Platform 2 Pose: [-2.34205026248273;9.97351656226392;6.41388016802297]
Robot Arm 2 end: [231833808.752759;-6695129.58373451;602481281.270992]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531990e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402611e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785395759505423;-9.97265434041995;-6.65284295638374]
Robot Arm 1 end: [224223952.511174;-1222454.09316907;-1064188214.62074]
Mobile Platform 2 Pose: [-2.34205026248272;9.97275738778247;6.41286653674347]
Robot Arm 2 end: [-849322481.410778;30954780.4077614;-700392087.847059]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692261e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.092891e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.78539575950542;-9.97322813791669;-6.650091457886]
Robot Arm 1 end: [-1198407552.33807;-9249532.47092853;1250085272.92758]
Mobile Platform 2 Pose: [-2.34205026248272;9.9742951561931;6.41521530696826]
Robot Arm 2 end: [1719847301.36331;-65595958.235896;89863859.1061979]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663859e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.038696e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.785395759505418;-9.97176377621066;-6.65747813788653]
Robot Arm 1 end: [2784765113.07278;31688938.9450267;-1070472746.67902]
Mobile Platform 2 Pose: [-2.34205026248272;9.97235760392319;6.41211107733479]
Robot Arm 2 end: [-2721109468.62108;112801022.285632;1248768617.03255]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984807e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.087005e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785395759505417;-9.97362810090819;-6.64792805266466]
Robot Arm 1 end: [-4519296452.82511;-55272504.1442334;-1018761030.15529]
Mobile Platform 2 Pose: [-2.34205026248272;9.9740896661459;6.41497102785137]
Robot Arm 2 end: [2358420748.51362;-107063306.020807;-4121053947.84962]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059253e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.365815e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785395759505417;-9.97178688315262;-6.65748565675353]
Robot Arm 1 end: [6283243710.90954;96846287.8917393;4907958069.40196]
Mobile Platform 2 Pose: [-2.34205026248272;9.9731860128444;6.41330490153774]
Robot Arm 2 end: [223951334.467611;21478789.6500689;7924105844.91941]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857436e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.453840e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785395759505416;-9.97319545986411;-6.65010797589848]
Robot Arm 1 end: [-4405407424.18948;-96706454.458224;-11934339250.721]
Mobile Platform 2 Pose: [-2.34205026248272;9.97306358164934;6.41331789637569]
Robot Arm 2 end: [-7144983525.28676;238128586.96705;-11234761430.7065]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905818e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.897975e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785395759505416;-9.97269674440125;-6.6529006943964]
Robot Arm 1 end: [-3339056330.23125;36141323.1049787;20624963161.8248]
Mobile Platform 2 Pose: [-2.34205026248272;9.97416061819841;6.4149086314987]
Robot Arm 2 end: [19521189813.672;-720071841.523354;7751736819.61952]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634522e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.328579e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785395759505416;-9.97221078186287;-6.65519569971042]
Robot Arm 1 end: [20909565967.75;157576364.729768;-28334794918.1931]
Mobile Platform 2 Pose: [-2.34205026248272;9.97233544965743;6.41206353021494]
Robot Arm 2 end: [-36350626872.0539;1460704925.94824;3743986693.49173]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.067006e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.828611e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785395759505416;-9.97350061344994;-6.64870124288541]
Robot Arm 1 end: [-51661904348.591;-518823161.245443;19244677888.5475]
Mobile Platform 2 Pose: [-2.34205026248272;9.97425495523018;6.41516783230907]
Robot Arm 2 end: [42473386125.5322;-1782354646.64987;-38538360381.9683]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.069100e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.488362e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785395759505416;-9.97163094324383;-6.65826420143815]
Robot Arm 1 end: [95318367953.7112;1243819824.8773;6906275485.49351]
Mobile Platform 2 Pose: [-2.34205026248272;9.97286582461743;6.4127639345118]
Robot Arm 2 end: [-29815024440.0302;1523185919.86301;92671086196.5104]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.776574e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.703994e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785395759505416;-9.97354092068509;-6.64840078576544]
Robot Arm 1 end: [-118441142440.619;-1701295057.78608;-90800579895.9708]
Mobile Platform 2 Pose: [-2.34205026248272;9.97339489892066;6.41380356925822]
Robot Arm 2 end: [-40283143637.6664;972905222.47683;-155557401693.365]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247229e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.540939e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.785395759505416;-9.97212038205831;-6.65588241297168]
Robot Arm 1 end: [109616991803.863;2180492262.93474;227692242306.19]
Mobile Platform 2 Pose: [-2.34205026248272;9.97394629808734;6.41447965713044]
Robot Arm 2 end: [195048945036.039;-6876406873.56457;167133575753.297]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064538e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.495303e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785395759505416;-9.97279446018671;-6.65223019544898]
Robot Arm 1 end: [30214649160.5421;-1170570647.16439;-411665428450.924]
Mobile Platform 2 Pose: [-2.34205026248272;9.97243109446631;6.41219757918991]
Robot Arm 2 end: [-434112450935.835;16901193473.6092;-97240620455.8234]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.742992e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.506674e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.785395759505416;-9.97311524771318;-6.65079308598589]
Robot Arm 1 end: [-384967396103.888;-2493714892.4171;538258099651.885]
Mobile Platform 2 Pose: [-2.34205026248272;9.97433582447052;6.41521973043583]
Robot Arm 2 end: [633941724086.478;-25410228701.6031;-283442171207.124]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155981e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.140362e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785395759505416;-9.97184705340311;-6.6571492965035]
Robot Arm 1 end: [994500192342.927;10672047461.3165;-548177559895.653]
Mobile Platform 2 Pose: [-2.34205026248272;9.9725953544226;6.41231540236763]
Robot Arm 2 end: [-721893140739.229;32109448828.9675;951430274270.117]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302894e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.521445e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.785395759505416;-9.97362257766595;-6.64808091593135]
Robot Arm 1 end: [-1757654973439.24;-20557175254.7154;-148855727148.759]
Mobile Platform 2 Pose: [-2.34205026248272;9.97371252871798;6.41426211828314]
Robot Arm 2 end: [113964890744.558;-11970484743.4791;-1933560306406.94]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531499e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446658e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.785395759505416;-9.9717166240456;-6.65793855975414]
Robot Arm 1 end: [2642776179657.44;38911534149.5466;1514108070319.16]
Mobile Platform 2 Pose: [-2.34205026248272;9.97366607943371;6.41395803694721]
Robot Arm 2 end: [1584698378731;-50944429445.6051;2720325694271.73]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.613168e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.280456e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785395759505416;-9.97330001690457;-6.64968226677358]
Robot Arm 1 end: [-2264490539395.88;-42507623039.2592;-4265041228791.61]
Mobile Platform 2 Pose: [-2.34205026248272;9.97263215354059;6.41249193374103]
Robot Arm 2 end: [-4634441243731.99;173314031487.842;-2773931953503.44]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408568e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.794593e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785395759505416;-9.97255494574646;-6.65373713134501]
Robot Arm 1 end: [-117437185369.813;27954739629.4674;7992132356025.85]
Mobile Platform 2 Pose: [-2.34205026248272;9.97432907867316;6.41512259072548]
Robot Arm 2 end: [8389400516413.57;-324840113426.669;-848912129734.985]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394165e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.099202e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785395759505416;-9.97234650773978;-6.65460270445044]
Robot Arm 1 end: [6422828483048.41;36499222927.5079;-11734872827668.7]
Mobile Platform 2 Pose: [-2.34205026248272;9.97240910236839;6.41200778686434]
Robot Arm 2 end: [-12174829742112.8;513478463006.481;8105676793077.61]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956358e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.353832e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785395759505416;-9.97343026918599;-6.64918950043352]
Robot Arm 1 end: [-18489668012544.5;-174728966747.596;10033363131815.6]
Mobile Platform 2 Pose: [-2.34205026248272;9.97398392643767;6.41464076408723]
Robot Arm 2 end: [8469526394667.29;-412419351360.871;-21755780948376]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260469e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.096677e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785395759505416;-9.9716496895779;-6.6582623599848]
Robot Arm 1 end: [36354131489193.8;456566887870.958;-2431103979703.03]
Mobile Platform 2 Pose: [-2.34205026248272;9.97334401926106;6.41338729944382]
Robot Arm 2 end: [7274433542483.73;-138261426753.112;37954246106049.2]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807924e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.758686e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785395759505416;-9.97358545605024;-6.64829065169193]
Robot Arm 1 end: [-49277299472707.2;-669320918682.578;-28117872456177.5]
Mobile Platform 2 Pose: [-2.34205026248272;9.97291270273507;6.4129055566664]
Robot Arm 2 end: [-42905951789738.2;1505060429419.01;-49360103248180.5]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620323e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.152196e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.785395759505416;-9.97200013525334;-6.65659579823289]
Robot Arm 1 end: [54075019278981.7;962357428814.919;80098521652256.6]
Mobile Platform 2 Pose: [-2.34205026248272;9.9742349547005;6.41488072690523]
Robot Arm 2 end: [100158724258586;-3755940670917.29;22692299836467.8]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640783e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.567068e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785395759505416;-9.97292968734542;-6.65164343713894]
Robot Arm 1 end: [-9557682345451.28;-699535479277.819;-156352247073947]
Mobile Platform 2 Pose: [-2.34205026248272;9.97233110465292;6.41187295745125]
Robot Arm 2 end: [-172833980360803;7045452908373.69;45519367023271.6]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711110e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.989371e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785395759505416;-9.97299183524696;-6.65154595430159]
Robot Arm 1 end: [-116068561914940;-530692168078.6;224772236757823]
Mobile Platform 2 Pose: [-2.34205026248272;9.97418667936388;6.41490184947955]
Robot Arm 2 end: [178159680708961;-7708738694625.42;-218247134013749]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373559e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.265690e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785395759505416;-9.9719470975901;-6.65673575814755]
Robot Arm 1 end: [346519650248897;3453491260376.13;-257982566046590]
Mobile Platform 2 Pose: [-2.34205026248272;9.97301397835515;6.41282257305421]
Robot Arm 2 end: [-71029699308138;4568592542428.93;469283353177951]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767804e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.486056e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785395759505416;-9.97360123981163;-6.64831637226524]
Robot Arm 1 end: [-671023621737340;-7504322543195.22;35884456084095.7]
Mobile Platform 2 Pose: [-2.34205026248272;9.973237072479;6.4133827583304]
Robot Arm 2 end: [-308980803912244;9317178830382.54;-727120300705251]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002527e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.098872e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.785395759505416;-9.97166632916939;-6.65828886878773]
Robot Arm 1 end: [1.0807628109392e+15;15286771754363.1;429582401735726]
Mobile Platform 2 Pose: [-2.34205026248272;9.97405731315536;6.41450691788664]
Robot Arm 2 end: [1.06866659027042e+15;-38594160282808.8;656745989304642]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.187230e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.310189e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785395759505416;-9.97339148569609;-6.64932526482758]
Robot Arm 1 end: [-1.06933468978255e+15;-18020405982767.6;-1.48932291594902e+15]
Mobile Platform 2 Pose: [-2.34205026248272;9.97237146627229;6.41192214697566]
Robot Arm 2 end: [-2.17355285659217e+15;85948818593337.8;-147178900859192]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546800e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.844514e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785395759505416;-9.97241328570759;-6.65456938542917]
Robot Arm 1 end: [400173571018971;15959795788005.6;3.03164537953523e+15]
Mobile Platform 2 Pose: [-2.34205026248272;9.97430827988795;6.41502362486731]
Robot Arm 2 end: [2.86661839208708e+15;-117176223705914;-1.84131560570002e+15]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.136119e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.367669e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785395759505416;-9.97248703514302;-6.65398600314439]
Robot Arm 1 end: [1.81854124294254e+15;4843084417064.74;-4.74341507863064e+15]
Mobile Platform 2 Pose: [-2.34205026248272;9.97271516216314;6.41232334211796]
Robot Arm 2 end: [-2.73726512844555e+15;127332097608038;5.14314754320504e+15]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235455e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602043e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785395759505416;-9.97334546893175;-6.64975107464499]
Robot Arm 1 end: [-6.44798064226871e+15;-56478300790207.3;4.78745332255572e+15]
Mobile Platform 2 Pose: [-2.34205026248272;9.97356577085354;6.4138614954565]
Robot Arm 2 end: [-849618824290270;-3792352425724.08;-9.48945137376386e+15]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.018100e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.082644e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785395759505416;-9.97169001688515;-6.65815098776741]
Robot Arm 1 end: [1.35927688409939e+16;163727707088920;-2.82683341134171e+15]
Mobile Platform 2 Pose: [-2.34205026248272;9.97380603653243;6.4140247048528]
Robot Arm 2 end: [9.73958640331615e+15;-330729884507682;1.18755654708841e+16]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924400e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.099283e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.785395759505416;-9.97361436268771;-6.64826276837224]
Robot Arm 1 end: [-2.00182427216594e+16;-258595010997363;-8.05522449116297e+15]
Mobile Platform 2 Pose: [-2.34205026248272;9.97252495788265;6.41214440372723]
Robot Arm 2 end: [-2.45110684398588e+16;935915430013885;-1.00294318832502e+16]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213735e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.274222e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.785395759505416;-9.97189295993101;-6.65724026629774]
Robot Arm 1 end: [2.48688540562017e+16;409886547672684;2.73266321276145e+16]
Mobile Platform 2 Pose: [-2.34205026248272;9.97434327349199;6.41499716862484]
Robot Arm 2 end: [3.99984335601683e+16;-1.5731917394861e+15;-1.03409961147889e+16]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673361e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.746297e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785395759505416;-9.97305746989628;-6.65109586220301]
Robot Arm 1 end: [-1.15380608873752e+16;-355792930347401;-5.83124524593367e+16]
Mobile Platform 2 Pose: [-2.340252126926;9.97248519853414;6.41194365642739]
Robot Arm 2 end: [-4.67756471403876e+16;2.03995233081259e+15;5.38985042391706e+16]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183063e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.640001e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.785395759505416;-9.97285991702411;-6.65233930162249]
Robot Arm 1 end: [-3.15127046579566e+16;-34800167880908.2;9.14560010281454e+16]
Mobile Platform 2 Pose: [-2.340252126926;9.97386067496786;6.41428125483841]
Robot Arm 2 end: [261550173403966;-460999509899580;-1.14630379303949e+17]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.344632e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.039590e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.785395759505416;-9.97206146932767;-6.65624996138884]
Robot Arm 1 end: [1.17279228219938e+17;1.05835351360222e+15;-1.15018934995192e+17]
Mobile Platform 2 Pose: [-2.340252126926;9.97371272481971;6.41380848769412]
Robot Arm 2 end: [1.1932684395624e+17;-4.01502046273124e+15;1.42172823256504e+17]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.852530e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.451674e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.785395759505416;-9.97356419819656;-6.64863342628523]
Robot Arm 1 end: [-2.51413534454922e+17;-2.68327390434457e+15;4.88995381469731e+16]
Mobile Platform 2 Pose: [-2.340252126926;9.97257963449578;6.41217742325511]
Robot Arm 2 end: [-3.08059994975675e+17;1.17590904237869e+16;-8.26537094281056e+16]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.176507e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.104588e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.785395759505416;-9.97163721577708;-6.65853078430895]
Robot Arm 1 end: [4.31429104610783e+17;5.8787394693779e+15;1.04355876841126e+17]
Mobile Platform 2 Pose: [-2.33845290105128;9.97440914646288;6.41505041678029]
Robot Arm 2 end: [4.58203791576483e+17;-1.82313143732165e+16;-1.87133299936878e+17]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.271320e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.301683e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.785395759505416;-9.97346885751633;-6.64904234064822]
Robot Arm 1 end: [-4.78475054375325e+17;-7.42774572469392e+15;-5.05932891709319e+17]
Mobile Platform 2 Pose: [-2.33845290105128;9.97264032501833;6.41210700432596]
Robot Arm 2 end: [-5.30334332938292e+17;2.34074806102252e+16;6.62585380839197e+17]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.031279e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.917421e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.785395759505416;-9.9722749304327;-6.65538143357907]
Robot Arm 1 end: [3.20038919527255e+17;8.02147936421491e+15;1.12522602780007e+18]
Mobile Platform 2 Pose: [-2.33665772016634;9.97380094485993;6.41412206629757]
Robot Arm 2 end: [1.02188657202346e+17;-9.28985732258651e+15;-1.36414782165041e+18]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.041626e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.756626e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.785395759505416;-9.97262916395713;-6.65336195014782]
Robot Arm 1 end: [4.42679963372005e+17;-1.56787707238954e+15;-1.8770319326848e+18]
Mobile Platform 2 Pose: [-2.33665772016634;9.97372069221397;6.41375977002531]
Robot Arm 2 end: [1.09808400006886e+18;-3.49635581399965e+16;1.92284916363573e+18]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.761692e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.072683e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.785395759505416;-9.97324708396637;-6.65038084242645]
Robot Arm 1 end: [-2.17833260402783e+18;-1.72044197728219e+16;2.15941813139233e+18]
Mobile Platform 2 Pose: [-2.33665772016634;9.97270346596444;6.41232404997223]
Robot Arm 2 end: [-3.24574718557194e+18;1.20802793727197e+17;-1.94262821024855e+18]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.312920e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.657656e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.785395759505416;-9.97175093548316;-6.65793527828146]
Robot Arm 1 end: [4.98030717556038e+18;5.72026992452517e+16;-1.77602716541591e+18]
Mobile Platform 2 Pose: [-2.33665772016634;9.9743989633259;6.4149506742854]
Robot Arm 2 end: [5.83726862647823e+18;-2.2523074095756e+17;-6.41104720171965e+17]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.280231e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.239848e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.785395759505416;-9.97362749628035;-6.6483178019184]
Robot Arm 1 end: [-7.96343546106811e+18;-9.81848744371956e+16;-2.00042417741688e+18]
Mobile Platform 2 Pose: [-2.33665772016634;9.97248087017957;6.41183660897319]
Robot Arm 2 end: [-8.34592513907596e+18;3.51562420075413e+17;5.76627032942339e+18]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.480569e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.867421e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.785395759505416;-9.97180145216402;-6.65780304425469]
Robot Arm 1 end: [1.09027931444636e+19;1.69575235856235e+17;8.97423443904575e+18]
Mobile Platform 2 Pose: [-2.33665772016634;9.97403781206798;6.41444500797815]
Robot Arm 2 end: [5.47990221535754e+18;-2.70348498017629e+17;-1.51621008094006e+19]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.497092e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.353164e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.785395759505416;-9.97317561687589;-6.65059885762754]
Robot Arm 1 end: [-7.30259802448623e+18;-1.66268526200845e+17;-2.13439154787766e+19]
Mobile Platform 2 Pose: [-2.33665772016634;9.97343996397773;6.41325454615493]
Robot Arm 2 end: [5.90137202438569e+18;-1.29535154603137e+17;2.58561789856318e+19]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.274507e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.667070e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.785395759505416;-9.97272188343121;-6.65316059601251]
Robot Arm 1 end: [-6.85933076722536e+18;5.2265323989131e+16;3.63564666834839e+19]
Mobile Platform 2 Pose: [-2.33665772016634;9.97294559706321;6.41267401278058]
Robot Arm 2 end: [-3.07173339441025e+19;1.08329451294511e+18;-3.25677760942558e+19]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.044596e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.450567e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.785395759505416;-9.97218739617243;-6.65570602985396]
Robot Arm 1 end: [3.82704698952244e+19;2.98440978854095e+17;-4.93264822867468e+19]
Mobile Platform 2 Pose: [-2.33665772016634;9.97432217134404;6.41474052030687]
Robot Arm 2 end: [6.88000527799549e+19;-2.58162258055346e+18;1.19694917177319e+19]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.287960e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.519372e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.785395759505416;-9.97351166821598;-6.64903045169853]
Robot Arm 1 end: [-9.23500818130868e+19;-9.36576541656426e+17;3.17969376991901e+19]
Mobile Platform 2 Pose: [-2.33665772016634;9.9724029152852;6.41169653003198]
Robot Arm 2 end: [-1.15023168506019e+20;4.69803321968786e+18;3.77037710977866e+19]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.768293e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.086420e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.785395759505416;-9.97162998427734;-6.65866108832624]
Robot Arm 1 end: [1.68520897809079e+20;2.2136448701139e+18;1.6439677485916e+19]
Mobile Platform 2 Pose: [-2.33665772016634;9.97422322318911;6.41467971153171]
Robot Arm 2 end: [1.10427785555844e+20;-4.83988816331061e+18;-1.54818855525841e+20]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.691431e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.403835e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.785395759505416;-9.97353142065439;-6.64883731150291]
Robot Arm 1 end: [-2.06024106223703e+20;-2.99148452578202e+18;-1.65996523859614e+20]
Mobile Platform 2 Pose: [-2.33665772016634;9.97315077907787;6.4127502966184]
Robot Arm 2 end: [-2.26593280497661e+19;2.09606267306826e+18;3.16998253872059e+20]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.125953e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.869186e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.785395759505416;-9.9721430803344;-6.65615720335631]
Robot Arm 1 end: [1.83572854102703e+20;3.74689189484047e+18;4.08097744118482e+20]
Mobile Platform 2 Pose: [-2.33665772016634;9.97322442102536;6.41307747930547]
Robot Arm 2 end: [-2.45428554229282e+20;7.79211352710371e+18;-4.6844401652481e+20]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.954748e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.919797e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.785395759505416;-9.97276979250095;-6.65274643939457]
Robot Arm 1 end: [7.10766608092347e+19;-1.85571366592906e+18;-7.28488069613456e+20]
Mobile Platform 2 Pose: [-2.33665772016634;9.97418114105673;6.41442815353724]
Robot Arm 2 end: [7.46767717977046e+20;-2.7167562803237e+19;3.6752571560375e+20]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.454245e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.944531e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.785395759505416;-9.97313630888145;-6.65107205799332]
Robot Arm 1 end: [-7.06034084190496e+20;-4.7549791456519e+18;9.35440077750454e+20]
Mobile Platform 2 Pose: [-2.33665772016634;9.97241453686637;6.41169620235845]
Robot Arm 2 end: [-1.44086153824514e+21;5.72945905552903e+19;4.68567459101751e+19]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.664897e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.212276e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.785395759505416;-9.97183095425915;-6.65762292443605]
Robot Arm 1 end: [1.78579949453259e+21;1.93832033509293e+19;-9.28292554404947e+20]
Mobile Platform 2 Pose: [-2.33665772016634;9.97434814814861;6.41481052318495]
Robot Arm 2 end: [1.75084078734331e+21;-7.24338564944086e+19;-1.40762808670742e+21]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.393394e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.762473e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.785395759505416;-9.9736248017989;-6.64845585287198]
Robot Arm 1 end: [-3.10735471164859e+21;-3.6632780395783e+19;-3.40243956296826e+20]
Mobile Platform 2 Pose: [-2.33665772016634;9.97287894447901;6.41228651274364]
Robot Arm 2 end: [-1.36601864849634e+21;6.70583105809479e+19;3.55401391064704e+21]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.770717e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.137087e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.785395759505416;-9.97172783300253;-6.65827327901818]
Robot Arm 1 end: [4.61105535209031e+21;6.8414645902639e+19;2.80012001453561e+21]
Mobile Platform 2 Pose: [-2.33665772016634;9.97351382115825;6.41349349918294]
Robot Arm 2 end: [-1.30132942923074e+21;2.68780966775285e+19;-6.10058004263314e+21]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.770571e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.779143e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.785395759505416;-9.97328232453949;-6.65016186015962]
Robot Arm 1 end: [-3.83217577731744e+21;-7.3638843224233e+19;-7.65681612934158e+21]
Mobile Platform 2 Pose: [-2.33665772016634;9.97398145098601;6.41402765287618]
Robot Arm 2 end: [7.29997792707521e+21;-2.54364185764932e+20;6.74623320799757e+21]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.800077e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.980629e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.785395759505416;-9.97258045541855;-6.65399576561493]
Robot Arm 1 end: [-5.77062488127258e+20;4.50565813821711e+19;1.41411191385155e+22]
Mobile Platform 2 Pose: [-2.33665772016634;9.97251458995945;6.41183168194719]
Robot Arm 2 end: [-1.66090771413037e+22;6.41893163753422e+20;-4.21084189370152e+21]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.252126e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.280858e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.785395759505416;-9.97232185877726;-6.65511934565333]
Robot Arm 1 end: [1.18857412458939e+22;7.21952852764099e+19;-2.05245211076025e+22]
Mobile Platform 2 Pose: [-2.33665772016634;9.97440798684369;6.41482962688771]
Robot Arm 2 end: [2.44798432964974e+22;-9.74832456106032e+20;-1.03850981193676e+22]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.299204e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.891465e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.785395759505416;-9.97344401105919;-6.64950499857303]
Robot Arm 1 end: [-3.31904936260721e+22;-3.1735986981802e+20;1.69415088120126e+22]
Mobile Platform 2 Pose: [-2.33665772016634;9.97265020459024;6.41190071491808]
Robot Arm 2 end: [-2.79782957405432e+22;1.23769678495739e+21;3.5977103668344e+22]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.090326e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408574e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.785395759505416;-9.97164480235267;-6.65867921557376]
Robot Arm 1 end: [6.44995673080534e+22;8.15768906348344e+20;-2.70677099552164e+21]
Mobile Platform 2 Pose: [-2.33665772016634;9.97378856365022;6.41388195695399]
Robot Arm 2 end: [4.53729703420684e+21;-4.62577584255955e+20;-7.33742672598106e+22]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.166395e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.686826e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.785395759505416;-9.97357870168865;-6.64871271838086]
Robot Arm 1 end: [-8.61116512593433e+22;-1.18081005867594e+21;-5.19481094869743e+22]
Mobile Platform 2 Pose: [-2.33665772016634;9.97373352753033;6.41356049556819]
Robot Arm 2 end: [6.04101073037917e+22;-1.93588776482805e+21;1.02417222927286e+23]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.104762e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.692976e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.785395759505416;-9.97202085636556;-6.65688112773086]
Robot Arm 1 end: [9.20424844790844e+22;1.66567534170936e+21;1.44272871251019e+23]
Mobile Platform 2 Pose: [-2.33665772016634;9.9726932321272;6.41208646488269]
Robot Arm 2 end: [-1.75745865005301e+23;6.55424600556884e+21;-1.02052563323489e+23]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.823265e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.276079e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.785395759505416;-9.97290602264531;-6.65215437309818]
Robot Arm 1 end: [-1.02749264397207e+22;-1.16136906683947e+21;-2.77569817814355e+23]
Mobile Platform 2 Pose: [-2.33665772016634;9.97440099444115;6.41473518007032]
Robot Arm 2 end: [3.13083768089223e+23;-1.20966450957193e+22;-3.86388065462389e+22]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.180253e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.607274e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.785395759505416;-9.97301470314603;-6.65181617871592]
Robot Arm 1 end: [-2.15767801364513e+23;-1.07643223373547e+21;3.9257753867863e+23]
Mobile Platform 2 Pose: [-2.33665772016634;9.97248676798682;6.41162388792284]
Robot Arm 2 end: [-4.43879293452626e+23;1.8730146381523e+22;3.15863275622215e+23]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.634714e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.680661e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.785395759505416;-9.97192811776027;-6.65722393952943]
Robot Arm 1 end: [6.25146334087578e+23;6.32276844803415e+21;-4.42161877113741e+23]
Mobile Platform 2 Pose: [-2.33665772016634;9.97402756596029;6.41420856454056]
Robot Arm 2 end: [2.83833927185449e+23;-1.41353474161588e+22;-8.18557569604149e+23]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.897878e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.257211e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.785395759505416;-9.97360629771767;-6.64867653190768]
Robot Arm 1 end: [-1.19017804480094e+24;-1.34186028617583e+22;3.41133680774392e+22]
Mobile Platform 2 Pose: [-2.33665772016634;9.97345381121527;6.41305619277383]
Robot Arm 2 end: [3.36485081816532e+23;-7.72933630646436e+21;1.38377073095272e+24]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.379777e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.295315e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.785395759505416;-9.97167388807899;-6.65864232013914]
Robot Arm 1 end: [1.89438255500109e+24;2.69809238424102e+22;8.09228742327948e+23]
Mobile Platform 2 Pose: [-2.33665772016634;9.97293297894167;6.4124328860006]
Robot Arm 2 end: [-1.67331189258742e+24;5.92061033861734e+22;-1.7280509255721e+24]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.310469e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.484538e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.785395759505416;-9.97337617270069;-6.64979235747711]
Robot Arm 1 end: [-1.83156122152379e+24;-3.13950439007714e+22;-2.68550429795353e+24]
Mobile Platform 2 Pose: [-2.33665772016634;9.97432735856813;6.41453016833241]
Robot Arm 2 end: [3.70384302500327e+24;-1.39170219085997e+23;5.92728208320098e+23]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.975095e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.370758e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.785395759505416;-9.97243861254182;-6.65482957042412]
Robot Arm 1 end: [5.6781192399202e+23;2.66135612962952e+22;5.38443510128924e+24]
Mobile Platform 2 Pose: [-2.33665772016634;9.97240464981683;6.41147724099754]
Robot Arm 2 end: [-6.14948166156585e+24;2.51504978838411e+23;2.10952119600794e+24]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.050429e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.398633e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.785395759505416;-9.97246169156503;-6.65450605173979]
Robot Arm 1 end: [3.42632044696949e+24;1.14581298158732e+22;-8.32961601895225e+24]
Mobile Platform 2 Pose: [-2.33665772016634;9.97421568512446;6.41444791424772]
Robot Arm 2 end: [5.82443207339267e+24;-2.56218009316371e+23;-8.39617224766843e+24]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.981210e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.887318e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.785395759505416;-9.97336178683761;-6.65005355000764]
Robot Arm 1 end: [-1.16325725935452e+25;-1.03447524087226e+23;8.18598028597493e+24]
Mobile Platform 2 Pose: [-2.33665772016634;9.97316446403467;6.4125510112733]
Robot Arm 2 end: [-9.75286280307008e+23;1.03202239778422e+23;1.70370785596014e+25]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.582838e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.157790e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.785395759505416;-9.97168130156722;-6.65858726773088]
Robot Arm 1 end: [2.42014711116651e+25;2.93806756903177e+23;-4.4110557744009e+24]
Mobile Platform 2 Pose: [-2.33665772016634;9.97321062657904;6.412834697577]
Robot Arm 2 end: [-1.35161013378935e+25;4.3239473711048e+23;-2.49971847038114e+25]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.296616e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.596967e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.785395759505416;-9.97361040734575;-6.64867010289038]
Robot Arm 1 end: [-3.51197245727157e+25;-4.57633825972112e+23;-1.5139470263549e+25]
Mobile Platform 2 Pose: [-2.33665772016634;9.97418931318088;6.41422243563539]
Robot Arm 2 end: [4.03743323019952e+25;-1.47126034760233e+24;1.91857848929117e+25]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.797174e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.781271e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.785395759505416;-9.97191119240794;-6.65753866500437]
Robot Arm 1 end: [4.2769535654996e+25;7.13495047538699e+23;4.9513802274173e+25]
Mobile Platform 2 Pose: [-2.33665772016634;9.97241193457184;6.41147020415431]
Robot Arm 2 end: [-7.73534884864773e+25;3.07984355680786e+24;3.56456506392016e+24]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.178170e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.425687e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.785395759505416;-9.9730352469907;-6.65159921656324]
Robot Arm 1 end: [-1.79586828035064e+25;-6.03067099557221e+23;-1.03857538123377e+26]
Mobile Platform 2 Pose: [-2.33665772016634;9.97434363889383;6.41458393228303]
Robot Arm 2 end: [9.30687693853792e+25;-3.85873145857122e+24;-7.68648669458368e+25]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.204098e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.374827e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.785395759505416;-9.9728842112431;-6.6526027967795]
Robot Arm 1 end: [-5.99986128669699e+25;-1.15699353495106e+23;1.60426409732431e+26]
Mobile Platform 2 Pose: [-2.33665772016634;9.97289120197616;6.41208440644539]
Robot Arm 2 end: [-7.06451809431077e+25;3.49819544853165e+24;1.91858828896571e+26]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.979717e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.454571e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.785395759505416;-9.97204005758214;-6.65675040818636]
Robot Arm 1 end: [2.12782041056322e+26;1.95979731853787e+24;-1.98751315278679e+26]
Mobile Platform 2 Pose: [-2.33665772016634;9.97350011040218;6.41325104841888]
Robot Arm 2 end: [-7.42003289181523e+25;1.61531148744284e+24;-3.26891915732763e+26]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.914877e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.060333e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.785395759505416;-9.97357207921335;-6.64897894583991]
Robot Arm 1 end: [-4.47435091015774e+26;-4.81676827771765e+24;7.54778057973753e+25]
Mobile Platform 2 Pose: [-2.33665772016634;9.97399224565676;6.41382573993888]
Robot Arm 2 end: [3.97028206182838e+26;-1.38703703316572e+25;3.56797578337031e+26]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.893736e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.729562e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.785395759505416;-9.97164092251405;-6.65890392933232]
Robot Arm 1 end: [7.59283573202056e+26;1.04146750397325e+25;2.04106519963623e+26]
Mobile Platform 2 Pose: [-2.33665772016634;9.97250790493874;6.41159945044827]
Robot Arm 2 end: [-8.95176276779007e+26;3.46471830623938e+25;-2.14236640301776e+26]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.015072e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.034664e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.785395759505416;-9.97345609538864;-6.64949601733533]
Robot Arm 1 end: [-8.26214308778515e+26;-1.29992200687476e+25;-9.17205388982727e+26]
Mobile Platform 2 Pose: [-2.33665772016634;9.97440665359111;6.4146084722495]
Robot Arm 2 end: [1.30764300342532e+27;-5.21556898708988e+25;-5.75354572783964e+26]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.295637e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.707553e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.785395759505416;-9.97229949807911;-6.65564607989857]
Robot Arm 1 end: [5.14201254484746e+26;1.36082816307068e+25;2.00638463584732e+27]
Mobile Platform 2 Pose: [-2.33665772016634;9.97265985441452;6.41169410341235]
Robot Arm 2 end: [-1.47514883627157e+27;6.54592623877308e+25;1.95287817306631e+27]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663432e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.389835e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.785395759505416;-9.97260369356128;-6.6538825006532]
Robot Arm 1 end: [8.65101607054422e+26;-1.70108963431812e+24;-3.30797263588112e+27]
Mobile Platform 2 Pose: [-2.33665772016634;9.97377607378189;6.41364166269735]
Robot Arm 2 end: [1.92400531433088e+26;-2.28791138479806e+25;-3.94588895882118e+27]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.460922e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.986235e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.785395759505416;-9.97326581321035;-6.65067125700805]
Robot Arm 1 end: [-3.95463407541213e+27;-3.19143789034409e+25;3.72387560789903e+27]
Mobile Platform 2 Pose: [-2.33665772016634;9.97374631487377;6.41336117689618]
Robot Arm 2 end: [3.32093726885917e+27;-1.07071832645022e+26;5.45345613315217e+27]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.699291e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.271961e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.785395759505416;-9.97173858634541;-6.65838996944732]
Robot Arm 1 end: [8.89997572825346e+27;1.03158612327193e+26;-2.92817859436016e+27]
Mobile Platform 2 Pose: [-2.33665772016634;9.97268311887817;6.41184905619502]
Robot Arm 2 end: [-9.51374536611632e+27;3.55508962083645e+26;-5.35699275643248e+27]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.460277e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.453943e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.785395759505416;-9.97362637449484;-6.64871024602618]
Robot Arm 1 end: [-1.4020523984733e+28;-1.74290417227837e+26;-3.89111557147828e+27]
Mobile Platform 2 Pose: [-2.33665772016634;9.97440287612624;6.41451944012298]
Robot Arm 2 end: [1.67890068723702e+28;-6.49559994244009e+26;-2.3011592354331e+27]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.942757e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.585118e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.785395759505416;-9.97181674009186;-6.6581167540284]
Robot Arm 1 end: [1.88899793210151e+28;2.96598093211297e+26;1.63850781914566e+28]
Mobile Platform 2 Pose: [-2.33665772016634;9.97249286631414;6.41141148340648]
Robot Arm 2 end: [-2.359976521774e+28;9.97581137518486e+26;1.72928221233119e+28]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.949027e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.805617e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.785395759505416;-9.97315521044865;-6.65109267759689]
Robot Arm 1 end: [-1.20281251320012e+28;-2.85251018340628e+26;-3.81447689028424e+28]
Mobile Platform 2 Pose: [-2.33665772016634;9.97401718070442;6.41397188508386]
Robot Arm 2 end: [1.46766720674964e+28;-7.3831367992149e+26;-4.41827575220561e+28]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.167520e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.898862e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.785395759505416;-9.97274715527141;-6.65341970428858]
Robot Arm 1 end: [-1.38274699876868e+28;7.15573746984283e+25;6.40280895799552e+28]
Mobile Platform 2 Pose: [-2.33665772016634;9.97346766386758;6.41285788107407]
Robot Arm 2 end: [1.91185760067762e+28;-4.565515389667e+26;7.40390603013079e+28]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.559899e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.123929e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.785395759505416;-9.97216405928299;-6.65621616011031]
Robot Arm 1 end: [6.99462153365755e+28;5.6288606825498e+26;-8.57575611289078e+28]
Mobile Platform 2 Pose: [-2.33665772016634;9.97292042274953;6.4121918444546]
Robot Arm 2 end: [-9.1121061186487e+28;3.23442149951206e+27;-9.16550226207148e+28]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.084887e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.938733e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.785395759505416;-9.97352234001894;-6.64936159634826]
Robot Arm 1 end: [-1.64954243574741e+29;-1.68911996417408e+27;5.21834727350861e+28]
Mobile Platform 2 Pose: [-2.33665772016634;9.97433240758615;6.41431959872517]
Robot Arm 2 end: [1.99356074401601e+29;-7.50087042347266e+27;2.913118796037e+28]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.218823e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.899673e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.785395759505416;-9.97162972980782;-6.65905442407841]
Robot Arm 1 end: [2.97689551898676e+29;3.93642625223165e+27;3.6629970511609e+28]
Mobile Platform 2 Pose: [-2.33665772016634;9.97240659530652;6.41125827724309]
Robot Arm 2 end: [-3.28688466333025e+29;1.34609299080513e+28;1.17826014397811e+29]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.269482e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.789263e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.785395759505416;-9.97352133293585;-6.64927690998206]
Robot Arm 1 end: [-3.57905624513114e+29;-5.25541918034701e+27;-3.03050500328627e+29]
Mobile Platform 2 Pose: [-2.33665772016634;9.9742079922684;6.41421585358005]
Robot Arm 2 end: [3.07027846226324e+29;-1.35578181341223e+28;-4.55228041862873e+29]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.262931e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.702150e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.785395759505416;-9.97216630984424;-6.65642921373893]
Robot Arm 1 end: [3.05943589397277e+29;6.42541572608019e+27;7.30798600033302e+29]
Mobile Platform 2 Pose: [-2.33665772016634;9.97317821393329;6.41235185749768]
Robot Arm 2 end: [-3.936714703456e+28;5.03759744834527e+27;9.15455941370524e+29]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.652935e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.125260e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.785395759505416;-9.97274474203833;-6.65326468561575]
Robot Arm 1 end: [1.57270272253801e+29;-2.893053267685e+27;-1.28814210108545e+30]
Mobile Platform 2 Pose: [-2.33665772016634;9.97319683102612;6.41259190275084]
Robot Arm 2 end: [-7.43811135396256e+29;2.39675700838137e+28;-1.33355767550233e+30]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.466843e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.267810e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.785395759505416;-9.97315722140019;-6.65135169044756]
Robot Arm 1 end: [-1.2926761120957e+30;-9.01819019706105e+27;1.62347114864538e+30]
Mobile Platform 2 Pose: [-2.33665772016634;9.97419736512668;6.41401654071953]
Robot Arm 2 end: [2.18233839554554e+30;-7.96555925555555e+28;1.00046221795063e+30]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.163801e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.741451e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.785395759505416;-9.97181525547812;-6.65809456473521]
Robot Arm 1 end: [3.20410986943403e+30;3.51645121875706e+28;-1.56648476004758e+30]
Mobile Platform 2 Pose: [-2.33665772016634;9.97240953290184;6.41124451013497]
Robot Arm 2 end: [-4.15189531516577e+30;1.65520650801639e+29;2.47850732029905e+29]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.520846e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.828418e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.785395759505416;-9.97362653387171;-6.64883334152566]
Robot Arm 1 end: [-5.48914633031087e+30;-6.52333000432875e+28;-7.39149702858734e+29]
Mobile Platform 2 Pose: [-2.33665772016634;9.97433897024204;6.41435707004562]
Robot Arm 2 end: [4.94561158326165e+30;-2.05507129927069e+29;-4.19553546866839e+30]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.389735e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.167094e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.785395759505416;-9.97173978931446;-6.65860419056689]
Robot Arm 1 end: [8.03520007243252e+30;1.20169711299854e+29;5.16719748104874e+30]
Mobile Platform 2 Pose: [-2.33665772016634;9.97290358173058;6.41188251385357]
Robot Arm 2 end: [-3.64695874977475e+30;1.8227970395591e+29;1.03547880531622e+31]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.594049e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.117104e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.785395759505416;-9.97326404192849;-6.65064453281517]
Robot Arm 1 end: [-6.46229785579616e+30;-1.27364803020597e+29;-1.37353281983945e+31]
Mobile Platform 2 Pose: [-2.33665772016634;9.97348633963794;6.41300849144347]
Robot Arm 2 end: [-4.21653322729062e+30;9.59973699416441e+28;-1.75124824361278e+31]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.530664e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.065334e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.785395759505416;-9.97260619719986;-6.6542531035946]
Robot Arm 1 end: [-1.68056479205131e+30;7.17949301499472e+28;2.49992323090003e+31]
Mobile Platform 2 Pose: [-2.33665772016634;9.97400294852476;6.41362370600943]
Robot Arm 2 end: [2.15858993060043e+31;-7.56029807223581e+29;1.88615315738918e+31]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.299825e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.008587e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.785395759505416;-9.97229714542872;-6.65563636261991]
Robot Arm 1 end: [2.19490222815373e+31;1.41322345352227e+29;-3.58583327685917e+31]
Mobile Platform 2 Pose: [-2.33665772016634;9.97250139043539;6.41136747373671]
Robot Arm 2 end: [-4.82372159697311e+31;1.86972806377184e+30;-1.08618113505834e+31]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.929384e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.832001e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.785395759505416;-9.97345741094038;-6.64982220194126]
Robot Arm 1 end: [-5.9528844178772e+31;-5.75745114175397e+29;2.85008138012685e+31]
Mobile Platform 2 Pose: [-2.33665772016634;9.9744051632145;6.4143870525573]
Robot Arm 2 end: [6.98341816223899e+31;-2.789851093967e+30;-3.18421451611008e+31]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.329918e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.293408e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.785395759505416;-9.97164057373672;-6.65909276050804]
Robot Arm 1 end: [1.14343912189671e+32;1.45636197506866e+30;-1.92589082059238e+30]
Mobile Platform 2 Pose: [-2.33665772016634;9.97266967353658;6.41148776956636]
Robot Arm 2 end: [-7.77291508212568e+31;3.46030187684482e+30;1.05971874578449e+32]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.912021e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.923805e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.785395759505416;-9.97357137421895;-6.64913778098036]
Robot Arm 1 end: [-1.50314683865547e+32;-2.08150206844568e+30;-9.57815945522134e+31]
Mobile Platform 2 Pose: [-2.33665772016634;9.97376347678806;6.41340118602277]
Robot Arm 2 end: [7.56816331251074e+30;-1.12262699929948e+30;-2.12160385742388e+32]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.610569e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.125974e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.785395759505416;-9.97204218653745;-6.65716329791262]
Robot Arm 1 end: [1.56204620271143e+32;2.87841837319913e+30;2.59611839774907e+32]
Mobile Platform 2 Pose: [-2.33665772016634;9.97375905163553;6.41316180978927]
Robot Arm 2 end: [1.82432569304514e+32;-5.91597674314446e+30;2.90294787048673e+32]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.904991e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.382920e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.785395759505416;-9.97288189959793;-6.65266770045426]
Robot Arm 1 end: [-6.26094133514475e+30;-1.91581730823462e+30;-4.92401766995059e+32]
Mobile Platform 2 Pose: [-2.33665772016634;9.97267312938652;6.41161182881264]
Robot Arm 2 end: [-5.14890302047032e+32;1.92779900408129e+31;-2.80972829594609e+32]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.762918e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.555272e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.785395759505416;-9.97303750066326;-6.65208665247046]
Robot Arm 1 end: [-4.00038515093867e+32;-2.14827416973482e+30;6.84858171600274e+32]
Mobile Platform 2 Pose: [-2.33665772016634;9.9744046073292;6.4143034523105]
Robot Arm 2 end: [9.00124155531439e+32;-3.48730764020846e+31;-1.35724697022934e+32]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.303750e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.789590e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.785395759505416;-9.97190943668123;-6.65771065037464]
Robot Arm 1 end: [1.12679146523465e+33;1.1558731173649e+31;-7.56061216766043e+32]
Mobile Platform 2 Pose: [-2.33665772016634;9.97249916492433;6.41119939550259]
Robot Arm 2 end: [-1.25429390965799e+33;5.31154587530702e+31;9.46243145882923e+32]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.865422e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.006339e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.785395759505416;-9.97361089101897;-6.64903908720346]
Robot Arm 1 end: [-2.109376769999e+33;-2.39763686634885e+31;7.73744376430383e+30]
Mobile Platform 2 Pose: [-2.33665772016634;9.97400665691936;6.41373497066899]
Robot Arm 2 end: [7.57535659173542e+32;-3.85212072866304e+31;-2.38435203719301e+33]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.037726e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.018213e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.785395759505416;-9.97168220512263;-6.65899192336469]
Robot Arm 1 end: [3.31694672542476e+33;4.7577946508429e+31;1.51866005527474e+33]
Mobile Platform 2 Pose: [-2.33665772016634;9.97348151906735;6.41265960672578]
Robot Arm 2 end: [1.08288241877227e+33;-2.67418631257587e+31;3.96053186931507e+33]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.108096e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.835470e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.785395759505416;-9.97336025583993;-6.65026260325373]
Robot Arm 1 end: [-3.12977219373261e+33;-5.46264953579662e+31;-4.83826463785397e+33]
Mobile Platform 2 Pose: [-2.33665772016634;9.97290793183961;6.41195089332012]
Robot Arm 2 end: [-4.96034852365978e+33;1.76620829972724e+32;-4.85939686301153e+33]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602838e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.018271e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.785395759505416;-9.97246427191435;-6.6550879555807]
Robot Arm 1 end: [7.52947361355238e+32;4.41497397108713e+31;9.55506385355305e+33]
Mobile Platform 2 Pose: [-2.33665772016634;9.9743373171145;6.4141088088885]
Robot Arm 2 end: [1.07280175976687e+34;-4.04193254186886e+32;1.41885822489082e+33]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.927281e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.723568e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.785395759505416;-9.9724361736325;-6.65502703657938]
Robot Arm 1 end: [6.43193669172255e+33;2.54518005870507e+31;-1.46130963050137e+34]
Mobile Platform 2 Pose: [-2.33665772016634;9.97240875258084;6.41103964036617]
Robot Arm 2 end: [-1.75639244158794e+34;7.20278150898083e+32;6.57066873175863e+33]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183487e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.640729e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.785395759505416;-9.97337781112956;-6.65035746271841]
Robot Arm 1 end: [-2.09653285699639e+34;-1.89173385581246e+32;1.39629677840353e+34]
Mobile Platform 2 Pose: [-2.33665772016634;9.97420014451163;6.41398352937254]
Robot Arm 2 end: [1.617486210887e+34;-7.17082536640378e+32;-2.46756282214054e+34]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.334820e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.930618e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.785395759505416;-9.97167318274542;-6.6590205579972]
Robot Arm 1 end: [4.30564613933117e+34;5.26771448454082e+32;-6.7393018422272e+33]
Mobile Platform 2 Pose: [-2.33665772016634;9.97319202603676;6.41215283140834]
Robot Arm 2 end: [-1.41385811289442e+33;2.43338374905278e+32;4.9179425436824e+34]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.295726e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.976385e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.785395759505416;-9.97360589795504;-6.64908033056555]
Robot Arm 1 end: [-6.15545254931467e+34;-8.09265668149784e+32;-2.83548709824928e+34]
Mobile Platform 2 Pose: [-2.33665772016634;9.97318303749237;6.41234909968915]
Robot Arm 2 end: [-4.09045896763144e+34;1.32712827037614e+33;-7.11242505585365e+34]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.189727e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.799059e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.785395759505416;-9.97193009875752;-6.65783359220633]
Robot Arm 1 end: [7.3401700261399e+34;1.24036091958681e+33;8.96137630817706e+34]
Mobile Platform 2 Pose: [-2.33665772016634;9.97420529525225;6.41381046564912]
Robot Arm 2 end: [1.17933347406622e+35;-4.31153847647405e+33;5.21101153166198e+34]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.785655e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.413595e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.785395759505416;-9.97301250499496;-6.65210527687837]
Robot Arm 1 end: [-2.73449876724478e+34;-1.0185884855388e+33;-1.84841860905143e+35]
Mobile Platform 2 Pose: [-2.33665772016634;9.97240733367369;6.41101912329343]
Robot Arm 2 end: [-2.22802962911253e+35;8.89372445162334e+33;1.63466417433347e+34]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.383495e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.636269e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.785395759505416;-9.97290852275515;-6.65286608690178]
Robot Arm 1 end: [-1.13648920296054e+35;-3.01086146752931e+32;2.811190177759e+35]
Mobile Platform 2 Pose: [-2.33665772016634;9.97433414160993;6.4141299354298]
Robot Arm 2 end: [2.6271884817759e+35;-1.09417316199644e+34;-2.28913462609108e+35]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.188101e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.037602e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.785395759505416;-9.97201883519041;-6.6572499406221]
Robot Arm 1 end: [3.85642285358243e+35;3.62117872941303e+33;-3.428455558039e+35]
Mobile Platform 2 Pose: [-2.33665772016634;9.97291608155732;6.4116808320831]
Robot Arm 2 end: [-1.87904227824806e+35;9.48645694503878e+33;5.58724522998453e+35]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.891573e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.801223e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.785395759505416;-9.9735795260417;-6.64932668717644]
Robot Arm 1 end: [-7.95682807834777e+35;-8.63968434641268e+33;1.13687120898152e+35]
Mobile Platform 2 Pose: [-2.33665772016634;9.97347251135478;6.41276583219084]
Robot Arm 2 end: [-2.38884461382227e+35;5.65307191827145e+33;-9.37995374691827e+35]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.292110e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.041911e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.785395759505416;-9.97164538008272;-6.65927327574845]
Robot Arm 1 end: [1.33501053528578e+36;1.84346649688463e+34;3.9571248613296e+35]
Mobile Platform 2 Pose: [-2.33665772016634;9.9740135574677;6.41342154735078]
Robot Arm 2 end: [1.17319699481739e+36;-4.11920328470113e+34;9.96597302878795e+35]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.155160e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.088303e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.785395759505416;-9.97344272698764;-6.64995286238613]
Robot Arm 1 end: [-1.42421681652659e+36;-2.27255966173606e+34;-1.66110049300361e+36]
Mobile Platform 2 Pose: [-2.33665772016634;9.97249504907467;6.41113575594297]
Robot Arm 2 end: [-2.59876130254754e+36;1.00877525558553e+35;-5.48535409147455e+35]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.054811e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.988055e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.785395759505416;-9.97232449612797;-6.65590843987712]
Robot Arm 1 end: [8.15870610287647e+35;2.30142090480465e+34;3.57452851235995e+36]
Mobile Platform 2 Pose: [-2.33665772016634;9.97440351485757;6.41416536616005]
Robot Arm 2 end: [3.72858154613096e+36;-1.49199307811415e+35;-1.76049995998936e+36]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.503162e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.191176e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.785395759505416;-9.97257794574141;-6.65440451394041]
Robot Arm 1 end: [1.67627183175921e+36;-1.07732304046958e+33;-5.8247547891164e+36]
Mobile Platform 2 Pose: [-2.33665772016634;9.97267966063497;6.41128171185806]
Robot Arm 2 end: [-4.09312133029679e+36;1.82825036171555e+35;5.74876290082239e+36]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.695490e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.019406e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.785395759505416;-9.97328430612832;-6.65096279715804]
Robot Arm 1 end: [-7.1707403000549e+36;-5.90532392138334e+34;6.4101596213457e+36]
Mobile Platform 2 Pose: [-2.33665772016634;9.97375077425838;6.41316052951165]
Robot Arm 2 end: [2.5741125548551e+35;-5.45595425819413e+34;-1.14051677063903e+37]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.444227e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.394614e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.785395759505416;-9.97172675667267;-6.65884206584004]
Robot Arm 1 end: [1.58921354389705e+37;1.8585580155218e+35;-4.79259897491199e+36]
Mobile Platform 2 Pose: [-2.33665772016634;9.97377173521228;6.41296239002031]
Robot Arm 2 end: [1.00149166039661e+37;-3.26555673043951e+35;1.54480139804177e+37]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446381e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.510932e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.785395759505416;-9.97362472088224;-6.64910546100361]
Robot Arm 1 end: [-2.46633019686905e+37;-3.09164638324495e+35;-7.50895881376377e+36]
Mobile Platform 2 Pose: [-2.33665772016634;9.97266326681927;6.41137478763775]
Robot Arm 2 end: [-2.78596354243661e+37;1.04509937130935e+36;-1.47243288820156e+37]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.149039e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.433317e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.785395759505416;-9.97183275149487;-6.65842675742148]
Robot Arm 1 end: [3.26758776881649e+37;5.18182275253092e+35;2.98727082082002e+37]
Mobile Platform 2 Pose: [-2.33665772016634;9.9744061870046;6.41408721451467]
Robot Arm 2 end: [4.82494224209348e+37;-1.8718777821653e+36;-7.94098939689009e+36]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.534759e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.990132e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.785395759505416;-9.9731342396034;-6.6515894400578]
Robot Arm 1 end: [-1.96645720612416e+37;-4.88236950504996e+35;-6.81204286841291e+37]
Mobile Platform 2 Pose: [-2.33665772016634;9.97250566352272;6.41098762420829]
Robot Arm 2 end: [-6.66401622282705e+37;2.82719975636674e+36;5.17507683796379e+37]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.365705e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.936630e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.785395759505416;-9.9727725398294;-6.65367811874504]
Robot Arm 1 end: [-2.74667930408649e+37;8.91994175554563e+34;1.12654453415949e+38]
Mobile Platform 2 Pose: [-2.33665772016634;9.97399599527188;6.41349782243451]
Robot Arm 2 end: [3.90232332449887e+37;-2.00748958943945e+36;-1.28647724130064e+38]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.832788e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.578246e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.785395759505416;-9.97214079762986;-6.65672595616009]
Robot Arm 1 end: [1.27663592393369e+38;1.05767509672759e+36;-1.48890620303243e+38]
Mobile Platform 2 Pose: [-2.33665772016634;9.97349537393468;6.41246136536593]
Robot Arm 2 end: [6.11614617307324e+37;-1.55534143174097e+36;2.11807253062542e+38]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.297545e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.309583e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.785395759505416;-9.97353261213808;-6.64969476605155]
Robot Arm 1 end: [-2.94405889438236e+38;-3.04351806514364e+36;8.49669478982419e+37]
Mobile Platform 2 Pose: [-2.33665772016634;9.9728955095833;6.41171003780283]
Robot Arm 2 end: [-2.69935537310456e+38;9.64066696977943e+36;-2.57531072684846e+38]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.290386e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.529709e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.785395759505416;-9.97163020081559;-6.65944410028097]
Robot Arm 1 end: [5.25406325258549e+38;6.99412512297904e+36;7.8274212645089e+37]
Mobile Platform 2 Pose: [-2.33665772016634;9.97434208587912;6.41389779623967]
Robot Arm 2 end: [5.77194996683153e+38;-2.17760309696229e+37;6.83502501278613e+37]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.545694e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.381250e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.785395759505416;-9.9735106455125;-6.6497196441795]
Robot Arm 1 end: [-6.20904217118841e+38;-9.22416423228625e+36;-5.52531306859741e+38]
Mobile Platform 2 Pose: [-2.33665772016634;9.9724111224156;6.41082133189431]
Robot Arm 2 end: [-9.38313788815636e+38;3.85319332844449e+37;3.65877846968013e+38]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.196897e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.623512e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.785395759505416;-9.97219006133332;-6.65669848639129]
Robot Arm 1 end: [5.07096205377544e+38;1.09946542763792e+37;1.30751321195954e+39]
Mobile Platform 2 Pose: [-2.33665772016634;9.97419214177706;6.4137509415255]
Robot Arm 2 end: [8.51590858216576e+38;-3.79088524554416e+37;-1.33721200911349e+39]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.160830e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.067181e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.785395759505416;-9.97271932133902;-6.65378487023653]
Robot Arm 1 end: [3.34562896126815e+38;-4.41243796134906e+36;-2.27593147568769e+39]
Mobile Platform 2 Pose: [-2.33665772016634;9.97320589757388;6.41195392906067]
Robot Arm 2 end: [-3.82420072616144e+37;1.16028910157732e+37;2.64139115696007e+39]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.374478e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.379559e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.785395759505416;-9.9731779648662;-6.65163208851516]
Robot Arm 1 end: [-2.36291229873981e+39;-1.70227415046267e+37;2.81349573789705e+39]
Mobile Platform 2 Pose: [-2.33665772016634;9.97316924914307;6.41210629331314]
Robot Arm 2 end: [-2.24797670619229e+39;7.34127479373032e+37;-3.79233422027555e+39]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.701807e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.683188e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.785395759505416;-9.97179998591547;-6.65856407085126]
Robot Arm 1 end: [5.7441647826102e+39;6.37224699408041e+37;-2.63314884479801e+39]
Mobile Platform 2 Pose: [-2.33665772016634;9.97421310192966;6.41360420729767]
Robot Arm 2 end: [6.37161856014643e+39;-2.3331292702889e+38;2.71088773719811e+39]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.469701e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.196771e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.785395759505416;-9.97362775877825;-6.64921346285381]
Robot Arm 1 end: [-9.68873415964462e+39;-1.16080049816271e+38;-1.55220166776021e+39]
Mobile Platform 2 Pose: [-2.33665772016634;9.97240533866622;6.41079404657065]
Robot Arm 2 end: [-1.19537126545688e+40;4.77774034094921e+38;1.04121569370103e+39]
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.362681e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 85)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',85,0)">第 85 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.548421e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.785395759505416;-9.97175250190354;-6.65893124543559]
Robot Arm 1 end: [1.398374301958e+40;2.10863850796753e+38;9.51577181278942e+39]
Mobile Platform 2 Pose: [-2.33665772016634;9.97432915243186;6.4139025274283]
Robot Arm 2 end: [1.39512766016976e+40;-5.82397314700278e+38;-1.24848773313482e+40]
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.242677e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 end: [-9.8543202540212;-9.82617087977973;1.50722002620516]
Robot Arm 1 togoal: 28.0583548054967
Mobile Platform 2 Pose: [-2.35629211821169;9.99866771778191;9.81844390808775]
Robot Arm 2 end: [9.85353116057372;9.82440257079388;1.50725286604989]
Robot Arm 1 togoal: 28.0565469332287

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.750911e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 end: [-9.72929347716611;-9.65232574912907;1.40200599186901]
Robot Arm 1 togoal: 27.8473174297274
Mobile Platform 2 Pose: [-2.35629199989922;9.99733167863975;9.63690091250698]
Robot Arm 2 end: [9.72764003222061;9.64801727528859;1.40568255499335]
Robot Arm 1 togoal: 27.8430900261454

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.448513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397808620236;-9.99576749061104;-9.48372476060934]
Robot Arm 1 end: [-9.69930061746444;-9.47951608220888;1.23897968990596]
Robot Arm 1 togoal: 27.7054782862447
Mobile Platform 2 Pose: [-2.35629188158151;9.99599115838722;9.45545425466046]
Robot Arm 2 end: [9.69154743997905;9.46812272384074;1.24887286425904]
Robot Arm 1 togoal: 27.69185613406

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.298220e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397690864383;-9.99434602103906;-9.31171183959808]
Robot Arm 1 end: [-9.77867309312711;-9.30834404725646;1.12386821017013]
Robot Arm 1 togoal: 27.6435484847823
Mobile Platform 2 Pose: [-2.35629176345133;9.9946427347804;9.27450987408015]
Robot Arm 2 end: [9.75805533204149;9.28464215428427;1.13383266427517]
Robot Arm 1 togoal: 27.6121001024671

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.274658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397573862388;-9.99291422087325;-9.1404729886008]
Robot Arm 1 end: [-9.85305702661775;-9.13781278677307;1.12021872025027]
Robot Arm 1 togoal: 27.5782591400891
Mobile Platform 2 Pose: [-2.3562916456849;9.99328288840858;9.09451173973663]
Robot Arm 2 end: [9.82638677830369;9.10209881083975;1.11812539745193]
Robot Arm 1 togoal: 27.5343076834585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.360940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397457530989;-9.99147370673166;-8.96989849346623]
Robot Arm 1 end: [-9.83163316704073;-8.96674749629692;1.10413295360329]
Robot Arm 1 togoal: 27.4445798436545
Mobile Platform 2 Pose: [-2.35629152809513;9.99191235930392;8.91540929020426]
Robot Arm 2 end: [9.81758164579213;8.92353965271873;1.09909329069962]
Robot Arm 1 togoal: 27.4046510750823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397341528709;-9.99002831581144;-8.79949883355178]
Robot Arm 1 end: [-9.88437209684222;-8.79686073268705;0.993019402149945]
Robot Arm 1 togoal: 27.3676632287343
Mobile Platform 2 Pose: [-2.3562914102482;9.99053428328419;8.73682081803963]
Robot Arm 2 end: [9.86446577996449;8.74321909110907;1.01042331330304]
Robot Arm 1 togoal: 27.3160383506411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397226048931;-9.98857513953434;-8.62967073274173]
Robot Arm 1 end: [-10.0240635535027;-8.62844670979544;1.06365451935044]
Robot Arm 1 togoal: 27.3530859850049
Mobile Platform 2 Pose: [-2.35629129212853;9.989146547524;8.55900549104408]
Robot Arm 2 end: [9.96949459698934;8.56148626854735;1.05871774204259]
Robot Arm 1 togoal: 27.2676216755822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785397111196129;-9.98711511781;-8.46062525935787]
Robot Arm 1 end: [-9.83059699093514;-8.4567607557185;1.11116984223786]
Robot Arm 1 togoal: 27.0937733689667
Mobile Platform 2 Pose: [-2.35611294347721;9.98774923945982;8.38210545889371]
Robot Arm 2 end: [9.84188312795569;8.38983282152121;1.07398937076324]
Robot Arm 1 togoal: 27.0570895657608

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396995929675;-9.98565931998688;-8.29072857539468]
Robot Arm 1 end: [-9.9514182120527;-8.2888018474312;0.775243010355489]
Robot Arm 1 togoal: 27.0757366506331
Mobile Platform 2 Pose: [-2.35592029797576;9.98635048523572;8.20503366189284]
Robot Arm 2 end: [9.97772932146049;8.20703298494811;0.847078563176535]
Robot Arm 1 togoal: 27.0381191849496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396877283108;-9.9841823282427;-8.12462828169497]
Robot Arm 1 end: [-10.2177197802578;-8.1250548188438;1.3488083098789]
Robot Arm 1 togoal: 27.1533323052119
Mobile Platform 2 Pose: [-2.35577921900815;9.98493699409107;8.03016687124559]
Robot Arm 2 end: [10.0962615669093;8.02818149538781;1.27734044156299]
Robot Arm 1 togoal: 26.9987856574457

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396758387117;-9.98269840344088;-7.9599482888563]
Robot Arm 1 end: [-9.14378697899978;-7.94738998499406;0.731100312735086]
Robot Arm 1 togoal: 26.2529489521983
Mobile Platform 2 Pose: [-2.35483103247575;9.9835183817679;7.85622900634236]
Robot Arm 2 end: [9.39762275457402;7.8815409854527;0.734858008213365]
Robot Arm 1 togoal: 26.3938495823259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396633784509;-9.98124455234098;-7.79314279536571]
Robot Arm 1 end: [-11.3193241028857;-7.8090058656407;0.799171001188807]
Robot Arm 1 togoal: 27.788402387144
Mobile Platform 2 Pose: [-2.35432504039524;9.98211893457859;7.68192212431694]
Robot Arm 2 end: [10.9161475424997;7.64626910383874;1.00162483204918]
Robot Arm 1 togoal: 27.3705277966976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396515982167;-9.9797235609508;-7.63794018922978]
Robot Arm 1 end: [-7.79091628714169;-7.60341009704214;2.47325654548136]
Robot Arm 1 togoal: 25.0460284657896
Mobile Platform 2 Pose: [-2.35394050278247;9.98066880720364;7.51575496172357]
Robot Arm 2 end: [8.27551794353869;7.58919318204061;1.83218748537062]
Robot Arm 1 togoal: 25.3667323585597

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396385817778;-9.97831591665116;-7.46636918684545]
Robot Arm 1 end: [-12.0397763896546;-7.50088735641034;-3.02355377029775]
Robot Arm 1 togoal: 28.5076440720531
Mobile Platform 2 Pose: [-2.35338524508733;9.97930446874298;7.33822049647279]
Robot Arm 2 end: [11.7786390920632;7.26133538794762;-1.62058027676331]
Robot Arm 1 togoal: 27.9666291625019

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396271346212;-9.97678818484225;-7.31817266015123]
Robot Arm 1 end: [-9.67949070656626;-7.29011847767282;9.12697151192097]
Robot Arm 1 togoal: 27.2778546458834
Mobile Platform 2 Pose: [-2.35253381463469;9.97782826396442;7.17880149935358]
Robot Arm 2 end: [8.99529668225712;7.23646868131034;6.68419895047153]
Robot Arm 1 togoal: 26.1643989430468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785396152232725;-9.9753590119579;-7.16418511934567]
Robot Arm 1 end: [-3.71065304442149;-7.11653477562845;-12.0805986259674]
Robot Arm 1 togoal: 25.8062251174348
Mobile Platform 2 Pose: [-2.35128873407243;9.97645061620864;7.01005676092163]
Robot Arm 2 end: [6.73070067231281;7.11615566913732;-8.72708967261007]
Robot Arm 1 togoal: 26.036560291531

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.78539602676002;-9.97430990779222;-7.00084677780109]
Robot Arm 1 end: [-30.2146663511324;-7.2121398652141;16.5803737834588]
Robot Arm 1 togoal: 46.2629645312805
Mobile Platform 2 Pose: [-2.35112007393582;9.97523756052196;6.84195672672722]
Robot Arm 2 end: [22.45503238003;6.37745698040564;14.6326643837437]
Robot Arm 1 togoal: 38.6453873941738

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.78539595660333;-9.97259808478388;-6.91581468798367]
Robot Arm 1 end: [34.9462430189383;-6.35580498868279;-7.65591218700211]
Robot Arm 1 togoal: 31.2096638058743
Mobile Platform 2 Pose: [-2.35096331232881;9.97363424068156;6.72931037875531]
Robot Arm 2 end: [-21.1701992423322;7.97087520050013;-14.5810482420882]
Robot Arm 1 togoal: 26.5895028944861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395855227853;-9.97409553094106;-6.77330520679708]
Robot Arm 1 end: [-81.3808996754909;-7.7273971175268;-24.1214617243481]
Robot Arm 1 togoal: 96.5518786836863
Mobile Platform 2 Pose: [-2.35032285253941;9.97448138167381;6.56763196263612]
Robot Arm 2 end: [70.6898651050778;4.13380860432413;4.90349738671968]
Robot Arm 1 togoal: 81.9881708840396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395822155724;-9.97228129699828;-6.73833367577277]
Robot Arm 1 end: [76.3619688737767;-5.31119500467831;100.001227701261]
Robot Arm 1 togoal: 119.735954722746
Mobile Platform 2 Pose: [-2.34796056339896;9.97259570408342;6.51358042968068]
Robot Arm 2 end: [-91.2251631187331;10.7268669823517;40.6486676478302]
Robot Arm 1 togoal: 92.5100238575688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395795540074;-9.97333331315079;-6.69737167009219]
Robot Arm 1 end: [-34.8235998712462;-7.7793971352972;-213.071694500175]
Robot Arm 1 togoal: 219.943826011911
Mobile Platform 2 Pose: [-2.34792567645637;9.97428681929086;6.47077485217457]
Robot Arm 2 end: [126.045223318501;1.39060570856027;-134.441016182821]
Robot Arm 1 togoal: 192.675163242921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395781195851;-9.97294909644746;-6.68023583759776]
Robot Arm 1 end: [-168.168969937109;-7.38861937940048;325.748462403528]
Robot Arm 1 togoal: 370.364676108567
Mobile Platform 2 Pose: [-2.34792565999521;9.97308986340165;6.44691957362477]
Robot Arm 2 end: [-48.9073610706576;9.89487936280774;304.55062218716]
Robot Arm 1 togoal: 306.164238429696

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395772638942;-9.97200303864331;-6.67347995257358]
Robot Arm 1 end: [506.203118389378;-1.47056340630546;-352.803833198251]
Robot Arm 1 togoal: 609.832104718315
Mobile Platform 2 Pose: [-2.34792564959693;9.97332802413241;6.43383109030762]
Robot Arm 2 end: [-169.353533967599;11.6848318015872;-497.273585021995]
Robot Arm 1 togoal: 524.079973635217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395767401621;-9.97358488642586;-6.65848980933812]
Robot Arm 1 end: [-1005.55320447591;-18.1123330918698;26.7196212930335]
Robot Arm 1 togoal: 1016.2546850609
Mobile Platform 2 Pose: [-2.34792564354262;9.97392952100869;6.42664720744187]
Robot Arm 2 end: [714.736602295144;-19.1266898847897;526.547935399897]
Robot Arm 1 togoal: 894.974704709563

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785395764311851;-9.9716500757212;-6.66429501601706]
Robot Arm 1 end: [1511.85265275677;15.2296209955371;813.676896185149]
Robot Arm 1 togoal: 1707.39156002266
Mobile Platform 2 Pose: [-2.34792564003077;9.97235433435571;6.41958369387758]
Robot Arm 2 end: [-1501.03808124001;65.7882408094979;-185.101874874906]
Robot Arm 1 togoal: 1504.58184043922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785395762440954;-9.97334587567019;-6.65299730614471]
Robot Arm 1 end: [-1351.19763284877;-31.2082417716928;-2373.62078413452]
Robot Arm 1 togoal: 2737.85602426511
Mobile Platform 2 Pose: [-2.3479256379155;9.97427870765685;6.41987659461397]
Robot Arm 2 end: [2173.77258061238;-81.434117981981;-1001.70482226191]
Robot Arm 1 togoal: 2404.25197670698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395761295572;-9.9725430019079;-6.65580126947701]
Robot Arm 1 end: [46.0170964404173;10.6125495616541;4517.87356091879]
Robot Arm 1 togoal: 4516.49621509536
Mobile Platform 2 Pose: [-2.34491612208808;9.97262168415919;6.41535288180544]
Robot Arm 2 end: [-2415.3304914276;114.775895003992;3302.71845734002]
Robot Arm 1 togoal: 4086.45181659023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395760593853;-9.97237811570277;-6.65550571971024]
Robot Arm 1 end: [3450.44579618475;11.4998902600568;-6706.85908148388]
Robot Arm 1 togoal: 7539.16658814525
Mobile Platform 2 Pose: [-2.34491612131046;9.97370799926671;6.41623236996486]
Robot Arm 2 end: [300.19710582389;-30.7603163020584;-6638.35954298826]
Robot Arm 1 togoal: 6647.15479437244

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395760172427;-9.97341831965915;-6.64975907402211]
Robot Arm 1 end: [-10301.8682420423;-102.703915394432;5930.5702818107]
Robot Arm 1 togoal: 11895.4173613172
Mobile Platform 2 Pose: [-2.34491612083973;9.97368529440544;6.41533946568863]
Robot Arm 2 end: [5595.94869326256;-174.738484328667;9232.33394121836]
Robot Arm 1 togoal: 10801.0049104459

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395759909198;-9.97165700587365;-6.65837171221346]
Robot Arm 1 end: [20469.364700502;248.680624240765;-1882.17744122425]
Robot Arm 1 togoal: 20547.2843024573
Mobile Platform 2 Pose: [-2.34491612054996;9.97262163498885;6.41343801335817]
Robot Arm 2 end: [-16030.4615292988;608.503197300834;-9257.67789894794]
Robot Arm 1 togoal: 18514.0621708768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785395759753156;-9.97359339940187;-6.64819520364775]
Robot Arm 1 end: [-28186.7050205592;-385.776150122854;-15181.8067871836]
Robot Arm 1 togoal: 32027.2423956621
Mobile Platform 2 Pose: [-2.34491612037833;9.97433271793233;6.41586893303319]
Robot Arm 2 end: [28720.2341350422;-1108.46782650863;-3373.18717338535]
Robot Arm 1 togoal: 28948.6034253076

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395759655636;-9.97198118931604;-6.65650177558254]
Robot Arm 1 end: [31681.8042510381;548.612475104918;44427.6510685911]
Robot Arm 1 togoal: 54562.5725271984
Mobile Platform 2 Pose: [-2.34491612027127;9.97241722232949;6.41261328034292]
Robot Arm 2 end: [-41241.8874055999;1751.7958495546;28475.6321200502]
Robot Arm 1 togoal: 50139.2806861102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785395759597111;-9.97295374746973;-6.65125334903681]
Robot Arm 1 end: [-7502.89198085192;-425.74635124906;-88010.3489874196]
Robot Arm 1 togoal: 88333.0214556313
Mobile Platform 2 Pose: [-2.34491612020772;9.97397264249966;6.41513462745058]
Robot Arm 2 end: [27868.9646172116;-1365.48436041609;-75059.3012694316]
Robot Arm 1 togoal: 80082.4640339086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785395759561428;-9.97296922404441;-6.6513492560343]
Robot Arm 1 end: [-62264.5275811783;-262.757453376312;128556.418974336]
Robot Arm 1 togoal: 142844.506283158
Mobile Platform 2 Pose: [-2.34491612016881;9.9733614035796;6.41387433409223]
Robot Arm 2 end: [27169.3169678692;-557.02126978798;129589.039010512]
Robot Arm 1 togoal: 132408.229831965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.785395759539366;-9.97196650325642;-6.6562907865685]
Robot Arm 1 end: [191892.30519246;1876.69467306697;-150202.203032048]
Robot Arm 1 togoal: 243687.222291343
Mobile Platform 2 Pose: [-2.34491612014505;9.9728973124992;6.41330828471596]
Robot Arm 2 end: [-149793.040977091;5291.57177951379;-166869.419094957]
Robot Arm 1 togoal: 224296.626536944

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.78539575952626;-9.97359580921727;-6.64798611087732]
Robot Arm 1 end: [-378054.201484501;-4200.35521888529;29548.7155349618]
Robot Arm 1 togoal: 379240.430650622
Mobile Platform 2 Pose: [-2.34491612013094;9.9742419688255;6.41530339316374]
Robot Arm 2 end: [344524.867727085;-12951.7876147333;72079.4466516572]
Robot Arm 1 togoal: 352231.447080794

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785395759518025;-9.97165958807069;-6.65794808034011]
Robot Arm 1 end: [615946.857563428;8646.93460776362;227091.874769013]
Robot Arm 1 togoal: 656523.295793428
Mobile Platform 2 Pose: [-2.34491612012213;9.97233239734067;6.41227058817521]
Robot Arm 2 end: [-589639.39424897;24110.1694484337;165847.411601265]
Robot Arm 1 togoal: 612984.065507379

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785395759513141;-9.97340627578782;-6.64887127655015]
Robot Arm 1 end: [-622984.004364802;-10337.6694508458;-825231.60165635]
Robot Arm 1 togoal: 1034039.80120047
Mobile Platform 2 Pose: [-2.34491612011693;9.97417841392491;6.41528020923039]
Robot Arm 2 end: [598884.99342176;-26036.0744832653;-758027.654480704]
Robot Arm 1 togoal: 966416.473668891

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785395759510112;-9.97238834370791;-6.65431341078414]
Robot Arm 1 end: [270262.908032517;9520.57005124866;1705517.45229766]
Robot Arm 1 togoal: 1726821.26381187
Mobile Platform 2 Pose: [-2.34491612011371;9.9730304785281;6.41323425561917]
Robot Arm 2 end: [-215539.554817087;14594.4124190905;1611769.02675466]
Robot Arm 1 togoal: 1626179.80877526

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785395759508273;-9.97251220481971;-6.6534707095392]
Robot Arm 1 end: [961279.375483982;1820.7010156125;-2698709.16569358]
Robot Arm 1 togoal: 2864800.42260463
Mobile Platform 2 Pose: [-2.34491612011177;9.97322024883593;6.41373937913076]
Robot Arm 2 end: [-1099076.52987224;33647.7318138541;-2476207.42626437]
Robot Arm 1 togoal: 2709370.92892582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785395759507166;-9.97332888203844;-6.64945225394588]
Robot Arm 1 end: [-3570605.95031385;-30789.6257131403;2793563.59556249]
Robot Arm 1 togoal: 4533677.97114149
Mobile Platform 2 Pose: [-2.34491612011061;9.97406833720511;6.4149086771807]
Robot Arm 2 end: [3699434.86658127;-134081.065470782;2188698.74011155]
Robot Arm 1 togoal: 4300495.65613286

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785395759506475;-9.97169930686996;-6.65771324223667]
Robot Arm 1 end: [7628563.17739578;91153.6043330109;-1782474.23032732]
Robot Arm 1 togoal: 7834561.28580509
Mobile Platform 2 Pose: [-2.34491612010988;9.97236667260013;6.41229421053375]
Robot Arm 2 end: [-7457364.95153067;295736.856837698;-382077.583762081]
Robot Arm 1 togoal: 7472990.9352778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785395759506066;-9.97361778204734;-6.64785910460435]
Robot Arm 1 end: [-11399858.0095542;-146024.253706467;-4269416.87606177]
Robot Arm 1 togoal: 12173998.9357517
Mobile Platform 2 Pose: [-2.34491612010946;9.97430425753556;6.4153993336237]
Robot Arm 2 end: [9725595.33053206;-398938.545592665;-6470328.48440241]
Robot Arm 1 togoal: 11688101.3602968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785395759505809;-9.97187540409097;-6.656938911363]
Robot Arm 1 end: [14432100.5488076;235168.310717649;15064225.0663815]
Robot Arm 1 togoal: 20863158.151759
Mobile Platform 2 Pose: [-2.34491612010919;9.97272907995291;6.41272424923882]
Robot Arm 2 end: [-9074595.8612331;425276.368129583;17780325.1325894]
Robot Arm 1 togoal: 19966694.3879214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785395759505656;-9.97307917302321;-6.65059553927168]
Robot Arm 1 end: [-7289790.19451002;-209245.292683304;-32717160.8703355]
Robot Arm 1 togoal: 33520108.7348239
Mobile Platform 2 Pose: [-2.34491612010903;9.97354965024378;6.41421586812971]
Robot Arm 2 end: [-3429662.03634849;7566.30804662834;-32492507.6155417]
Robot Arm 1 togoal: 32673012.1121257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785395759505562;-9.97283566172529;-6.65207568829744]
Robot Arm 1 end: [-16454813.889171;-2516.39569169932;52085162.1598021]
Robot Arm 1 togoal: 54622570.3750394
Mobile Platform 2 Pose: [-2.34491612010894;9.97382041978594;6.4144289610568]
Robot Arm 2 end: [34059810.2749496;-1163360.15659633;40117315.1970981]
Robot Arm 1 togoal: 52638613.1783045
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.036872e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782950e-16。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.785395759505504;-9.97208304294275;-6.65574885789258]
Robot Arm 1 end: [64570625.6775219;570222.461444526;-66454000.4325242]
Robot Arm 1 togoal: 92659721.1676379
Mobile Platform 2 Pose: [-2.34491612010888;9.9725147777214;6.41250694729402]
Robot Arm 2 end: [-84572721.5133523;3240035.17117491;-33015866.9748899]
Robot Arm 1 togoal: 90846513.4056881
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.811171e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.140863e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.78539575950547;-9.97355589919412;-6.64829011432792]
Robot Arm 1 end: [-141162822.240169;-1493574.75355871;31103006.8913202]
Robot Arm 1 togoal: 144556469.742964
Mobile Platform 2 Pose: [-2.34491612010884;9.97434370756073;6.41537964272533]
Robot Arm 2 end: [136524646.893435;-5385880.93221938;-37671258.7067798]
Robot Arm 1 togoal: 141729013.941104
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.563931e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.131981e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785395759505448;-9.97163424244322;-6.65815397495836]
Robot Arm 1 end: [244914598.485787;3315570.56939086;52795452.3080213]
Robot Arm 1 togoal: 250562383.886442
Mobile Platform 2 Pose: [-2.34491612010882;9.97249480343588;6.41233693828938]
Robot Arm 2 end: [-176949383.013681;7651196.4315636;169040853.598933]
Robot Arm 1 togoal: 244835518.748867
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.056079e-17。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.165216e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785395759505435;-9.97348102378628;-6.64859180451582]
Robot Arm 1 end: [-276643625.838955;-4239890.24996092;-278783330.992382]
Robot Arm 1 togoal: 392771975.186103
Mobile Platform 2 Pose: [-2.34491612010881;9.97384882163423;6.41464193948693]
Robot Arm 2 end: [80947076.9346403;-4647010.30178824;-385355455.890376]
Robot Arm 1 togoal: 393792907.569118
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.231848e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.659135e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785395759505427;-9.97225080614539;-6.65511445526811]
Robot Arm 1 end: [197163137.396726;4714814.6146564;630522286.728663]
Robot Arm 1 togoal: 660646562.339429
Mobile Platform 2 Pose: [-2.3449161201088;9.9735166819651;6.41387758793386]
Robot Arm 2 end: [231833818.744456;-6706672.37339546;602481432.045765]
Robot Arm 1 togoal: 645581735.799327
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.531990e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.402591e-18。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.785395759505423;-9.97265434030515;-6.65284295629283]
Robot Arm 1 end: [224224063.820261;-1222452.73379257;-1064188219.93896]
Robot Arm 1 togoal: 1087554362.5129
Mobile Platform 2 Pose: [-2.34491612010879;9.97275750677622;6.4128639663833]
Robot Arm 2 end: [-849321580.262644;30997194.9334751;-700392017.479337]
Robot Arm 1 togoal: 1101297841.96032
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.692260e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.092782e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.78539575950542;-9.97322813797382;-6.65009145689996]
Robot Arm 1 end: [-1198407716.47578;-9249534.68807256;1250085169.8901]
Robot Arm 1 togoal: 1731756207.18685
Mobile Platform 2 Pose: [-2.34491612010879;9.97429527566705;6.41521272518635]
Robot Arm 2 end: [1719844727.14035;-65681887.535413;89863083.6437838]
Robot Arm 1 togoal: 1723442892.13032
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663859e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.038562e-19。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785395759505418;-9.97176377613675;-6.65747813759201]
Robot Arm 1 end: [2784765303.62496;31688942.4273821;-1070472469.70915]
Robot Arm 1 togoal: 2983594014.05051
Mobile Platform 2 Pose: [-2.34491612010879;9.97235772338106;6.41210850606568]
Robot Arm 2 end: [-2721104174.59968;112937166.736485;1248770484.82696]
Robot Arm 1 togoal: 2996095859.69259
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.984807e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.086804e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785395759505417;-9.97362810088841;-6.64792805207277]
Robot Arm 1 end: [-4519296462.68536;-55272506.3077974;-1018761573.15517]
Robot Arm 1 togoal: 4633030412.71283
Mobile Platform 2 Pose: [-2.34491612010879;9.97408978516793;6.41496844925555]
Robot Arm 2 end: [2358414072.49916;-107181451.15213;-4121056719.02637]
Robot Arm 1 togoal: 4749390838.81569
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.059253e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.365773e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785395759505416;-9.97178688319742;-6.65748565585436]
Robot Arm 1 end: [6283243268.46621;96846285.2115949;4907958971.56652]
Robot Arm 1 togoal: 7973492728.89452
Mobile Platform 2 Pose: [-2.34491612010879;9.97318613274791;6.41330232204735]
Robot Arm 2 end: [223957351.463683;21468113.055005;7924108276.40334]
Robot Arm 1 togoal: 7927301541.64203
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.857436e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.453840e-20。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785395759505416;-9.97319545975326;-6.65010797578431]
Robot Arm 1 end: [-4405406132.81335;-96706442.9876357;-11934340172.987]
Robot Arm 1 togoal: 12721848561.5127
Mobile Platform 2 Pose: [-2.34491612010879;9.97306370039904;6.41331532264607]
Robot Arm 2 end: [-7144980838.53299;238485035.27164;-11234760939.8591]
Robot Arm 1 togoal: 13316436443.9921
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.905819e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.897919e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785395759505416;-9.97269674450398;-6.65290069318509]
Robot Arm 1 end: [-3339059194.12204;36141289.8157471;20624963277.1304]
Robot Arm 1 togoal: 20893533274.2209
Mobile Platform 2 Pose: [-2.34491612010879;9.974160737886;6.41490604780134]
Robot Arm 2 end: [19521164873.0094;-721046753.753739;7751726096.67811]
Robot Arm 1 togoal: 21016304248.1464
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.634522e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.328534e-21。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785395759505415;-9.97221078172521;-6.65519569974022]
Robot Arm 1 end: [20909570354.3091;157576427.748117;-28334793194.3588]
Robot Arm 1 togoal: 35214989248.3913
Mobile Platform 2 Pose: [-2.34491612010879;9.97233556891915;6.41206095834115]
Robot Arm 2 end: [-36350561742.7049;1462522713.03721;3744013225.27862]
Robot Arm 1 togoal: 36572119791.1505
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.067005e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.828356e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735006134901;-6.64870124197991]
Robot Arm 1 end: [-51661908679.1099;-518823233.047553;19244671359.4689]
Robot Arm 1 togoal: 55132380346.1711
Mobile Platform 2 Pose: [-2.34491612010879;9.97425507431096;6.41516525067222]
Robot Arm 2 end: [42473281614.2688;-1784479299.15491;-38538401982.2305]
Robot Arm 1 togoal: 57379198719.8243
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.069100e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.488324e-22。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163094321734;-6.65826420090662]
Robot Arm 1 end: [95318370181.4156;1243819900.49699;6906289269.41555]
Robot Arm 1 togoal: 95576333950.9674
Mobile Platform 2 Pose: [-2.34491612010879;9.97286594464484;6.41276135600837]
Robot Arm 2 end: [-29814880291.5135;1524681541.13709;92671132649.4721]
Robot Arm 1 togoal: 97361134783.2774
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.776572e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.703920e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.785395759505415;-9.97354092061521;-6.64840078543374]
Robot Arm 1 end: [-118441134054.26;-1701295043.17097;-90800600255.3807]
Robot Arm 1 togoal: 149251283580.074
Mobile Platform 2 Pose: [-2.34491612010879;9.97339501746962;6.41380099189796]
Robot Arm 2 end: [-40283227931.6413;974909475.782541;-155557411900.306]
Robot Arm 1 togoal: 160691621741.418
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.247228e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.540910e-23。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785395759505415;-9.97212038216775;-6.6558824117393]
Robot Arm 1 end: [109616959666.912;2180491948.448;227692267060.857]
Robot Arm 1 togoal: 252714069390.96
Mobile Platform 2 Pose: [-2.34491612010879;9.97394641806173;6.4144770722352]
Robot Arm 2 end: [195048788763.299;-6886143673.68631;167133441308.577]
Robot Arm 1 togoal: 256953373551.754
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.064539e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.495110e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.785395759505415;-9.97279446002223;-6.65223019561338]
Robot Arm 1 end: [30214712111.2733;-1170569927.31384;-411665441464.086]
Robot Arm 1 togoal: 412774435689.05
Mobile Platform 2 Pose: [-2.34491612010879;9.97243121340632;6.41219500564025]
Robot Arm 2 end: [-434111785615.178;16922893134.7025;-97240260737.5394]
Robot Arm 1 togoal: 445191076979.653
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.742991e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.506543e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785395759505415;-9.97311524782296;-6.65079308472694]
Robot Arm 1 end: [-384967498587.791;-2493716212.91044;538258055866.21]
Robot Arm 1 togoal: 661761232092.419
Mobile Platform 2 Pose: [-2.34491612010879;9.97433594371882;6.41521714608852]
Robot Arm 2 end: [633940326562.7;-25441917129.2371;-283442805173.996]
Robot Arm 1 togoal: 694886647308.444
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.155980e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.140324e-24。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.785395759505415;-9.97184705328127;-6.6571492964578]
Robot Arm 1 end: [994500322434.368;10672049662.7146;-548177423360.959]
Robot Arm 1 togoal: 1135624617305.51
Mobile Platform 2 Pose: [-2.34491612010879;9.97259547443013;6.41231282476984]
Robot Arm 2 end: [-721890699054.245;32145582013.3721;951431148327.536]
Robot Arm 1 togoal: 1194730408843.42
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.302894e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.521374e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362257766344;-6.64808091524349]
Robot Arm 1 end: [-1757655023760.11;-20557176936.3223;-148856025782.999]
Robot Arm 1 togoal: 1764066862825.97
Mobile Platform 2 Pose: [-2.34491612010879;9.97371264716986;6.41425953736699]
Robot Arm 2 end: [113962445307.596;-11976281811.4036;-1933560794706.04]
Robot Arm 1 togoal: 1936953333743.14
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.531498e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446656e-25。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785395759505415;-9.97171662409789;-6.6579385588234]
Robot Arm 1 end: [2642776019577.2;38911533827.7774;1514108597637.71]
Robot Arm 1 togoal: 3046030866964.3
Mobile Platform 2 Pose: [-2.34491612010879;9.9736661997239;6.41395545165976]
Robot Arm 2 end: [1584698966892.52;-51023487849.503;2720324323798.64]
Robot Arm 1 togoal: 3148656640323.85
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.613167e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.280359e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785395759505415;-9.97330001676718;-6.64968226679352]
Robot Arm 1 end: [-2264489920426.13;-42507618249.7042;-4265041826178.22]
Robot Arm 1 togoal: 4829109988023.63
Mobile Platform 2 Pose: [-2.34491612010879;9.97263227208922;6.41248935755105]
Robot Arm 2 end: [-4634436187017.38;173545594825.263;-2773927485520.28]
Robot Arm 1 togoal: 5403960634559.03
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408568e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.794540e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785395759505415;-9.97255494590405;-6.6537371298561]
Robot Arm 1 end: [-117438706260.762;27954722571.6671;7992132659880.02]
Robot Arm 1 togoal: 7993044336755.59
Mobile Platform 2 Pose: [-2.34491612010879;9.97432919819286;6.41512000403254]
Robot Arm 2 end: [8389384335389.14;-325259277964.473;-848921503341.632]
Robot Arm 1 togoal: 8438496954065.6
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.394164e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.099154e-26。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785395759505415;-9.97234650754405;-6.65460270477929]
Robot Arm 1 end: [6422830923487.38;36499256220.241;-11734872273325.1]
Robot Arm 1 togoal: 13377642413316.8
Mobile Platform 2 Pose: [-2.34491612010879;9.972409222173;6.41200520962739]
Robot Arm 2 end: [-12174795511419.7;514087425991.598;8105691846172.48]
Robot Arm 1 togoal: 14635305665796.5
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.956357e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.353765e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785395759505415;-9.97343026926908;-6.64918949930142]
Robot Arm 1 end: [-18489670798798.9;-174729008978.264;10033360037270.8]
Robot Arm 1 togoal: 21037270975874.7
Mobile Platform 2 Pose: [-2.34491612010879;9.97398404492864;6.41463817990479]
Robot Arm 2 end: [8469481743100.8;-412843540388.855;-21755793270609.7]
Robot Arm 1 togoal: 23349884402696.5
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.260469e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.096664e-27。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164968951921;-6.65826235962286]
Robot Arm 1 end: [36354133746698;456566941242.037;-2431097058758.44]
Robot Arm 1 togoal: 36438190497787.1
Mobile Platform 2 Pose: [-2.34491612010879;9.97334413982474;6.41338471448937]
Robot Arm 2 end: [7274475700797.12;-138623493083.222;37954238157873.4]
Robot Arm 1 togoal: 38645328402523.9
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.807923e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.758641e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.785395759505415;-9.97358545598195;-6.64829065134618]
Robot Arm 1 end: [-49277296667592.4;-669320930510.06;-28117883532900.5]
Robot Arm 1 togoal: 56739010669421.9
Mobile Platform 2 Pose: [-2.34491612010879;9.97291282090241;6.41290297715298]
Robot Arm 2 end: [-42905940684857.2;1507203202404.03;-49360054546239]
Robot Arm 1 togoal: 65418700631798.2
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.620323e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.152170e-28。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785395759505415;-9.97200013539397;-6.65659579684792]
Robot Arm 1 end: [54075004909353.7;962357299743.789;80098536844721.1]
Robot Arm 1 togoal: 96647855083197.8
Mobile Platform 2 Pose: [-2.34491612010879;9.97423507458558;6.4148781383287]
Robot Arm 2 end: [100158569123664;-3760943464511.28;22692169277991.1]
Robot Arm 1 togoal: 102765841655610
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.640783e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.566829e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785395759505415;-9.97292968713781;-6.65164343752433]
Robot Arm 1 end: [-9557651571534.47;-699535151799.742;-156352258263789]
Robot Arm 1 togoal: 156645672513411
Mobile Platform 2 Pose: [-2.34491612010879;9.97233122406952;6.41187037968343]
Robot Arm 2 end: [-172833568385878;7054094654568.25;45519601594778.4]
Robot Arm 1 togoal: 178866533319499
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.711110e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.989295e-29。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785395759505415;-9.97299183541534;-6.65154595274172]
Robot Arm 1 end: [-116068617086611;-530692851999.144;224772222413010]
Robot Arm 1 togoal: 252971851153022
Mobile Platform 2 Pose: [-2.34491612010879;9.97418679803417;6.41489926239743]
Robot Arm 2 end: [178159009652449;-7717647959040.32;-218247380403272]
Robot Arm 1 togoal: 281837034229456
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.373557e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.265559e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785395759505415;-9.97194709741018;-6.65673575840169]
Robot Arm 1 end: [346519725077500;3453492454036.58;-257982507953210]
Robot Arm 1 togoal: 432021782885699
Mobile Platform 2 Pose: [-2.34491612010879;9.97301409906376;6.41281998887607]
Robot Arm 2 end: [-71028770319143.4;4572156097563.22;469283436157797]
Robot Arm 1 togoal: 474650328427672
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.767804e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.486042e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360123983821;-6.64831637142007]
Robot Arm 1 end: [-671023667973013;-7504323596951.81;35884310965133.5]
Robot Arm 1 togoal: 672024375767769
Mobile Platform 2 Pose: [-2.34491612010879;9.97323719036162;6.41338017518945]
Robot Arm 2 end: [-308981364914464;9332597091247.7;-727119876846532]
Robot Arm 1 togoal: 790101193859486
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.002526e-30。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.098731e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785395759505415;-9.97166632921239;-6.65828886791101]
Robot Arm 1 end: [1.08076277256754e+15;15286772154410.1;429582672638706]
Robot Arm 1 togoal: 1.16310925050151e+15
Mobile Platform 2 Pose: [-2.34491612010879;9.97405743347681;6.41450432802991]
Robot Arm 2 end: [1.06866559278542e+15;-38647528412880.7;656744321326982]
Robot Arm 1 togoal: 1.25493142611503e+15
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.187228e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.310095e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785395759505415;-9.97339148554344;-6.64932526492237]
Robot Arm 1 end: [-1.06933442838942e+15;-18020404320814.6;-1.48932325070855e+15]
Robot Arm 1 togoal: 1.83354427266208e+15
Mobile Platform 2 Pose: [-2.34491612010879;9.97237158515446;6.411919567628]
Robot Arm 2 end: [-2.17354865991499e+15;86057471073682.8;-147175582423021]
Robot Arm 1 togoal: 2.18022483184817e+15
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.546799e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.844439e-31。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785395759505415;-9.97241328591609;-6.65456938368534]
Robot Arm 1 end: [400172850492457;15959788004132.5;3.03164564220283e+15]
Robot Arm 1 togoal: 3.05798435656427e+15
Mobile Platform 2 Pose: [-2.34491612010879;9.9743083988659;6.41502103524524]
Robot Arm 2 end: [2.86660956757743e+15;-117319481816579;-1.84131990361794e+15]
Robot Arm 1 togoal: 3.40905753269137e+15
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.136121e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.367542e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724870348897;-6.65398600376929]
Robot Arm 1 end: [1.81854245989743e+15;4843100162471.65;-4.7434149825572e+15]
Robot Arm 1 togoal: 5.08006947106209e+15
Mobile Platform 2 Pose: [-2.34491612010879;9.97271528281216;6.41232075873245]
Robot Arm 2 end: [-2.73724979846041e+15;127469008721580;5.14315146803967e+15]
Robot Arm 1 togoal: 5.82758885222306e+15
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.235454e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602019e-32。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785395759505415;-9.97334546906772;-6.64975107323676]
Robot Arm 1 end: [-6.44798221205574e+15;-56478323064475;4.78745201519743e+15]
Robot Arm 1 togoal: 8.0311494324151e+15
Mobile Platform 2 Pose: [-2.34491612010879;9.97356588861737;6.41385890875389]
Robot Arm 2 end: [-849634014379390;-3750153467329.71;-9.48944950449738e+15]
Robot Arm 1 togoal: 9.52741013709639e+15
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.018100e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.082658e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.785395759505415;-9.97169001677896;-6.65815098765241]
Robot Arm 1 end: [1.35927704162976e+16;163727738783727;-2.82683026995772e+15]
Robot Arm 1 togoal: 1.38845663863792e+16
Mobile Platform 2 Pose: [-2.34491612010879;9.97380615731214;6.41402211442818]
Robot Arm 2 end: [9.73958898349679e+15;-331216212516081;1.18755466849481e+16]
Robot Arm 1 togoal: 1.53622233681609e+16
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.924401e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.099238e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361436263305;-6.64826276794917]
Robot Arm 1 end: [-2.00182421168577e+16;-258595025491571;-8.05522997285552e+15]
Robot Arm 1 togoal: 2.1579703861544e+16
Mobile Platform 2 Pose: [-2.34491612010879;9.97252507615934;6.41214182180891]
Robot Arm 2 end: [-2.45110356261286e+16;937140465559877;-1.00293892402812e+16]
Robot Arm 1 togoal: 2.65001461930044e+16
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.213735e-33。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.273961e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785395759505415;-9.97189296008914;-6.6572402648311]
Robot Arm 1 end: [2.48688483510241e+16;409886502976892;2.73266403871739e+16]
Robot Arm 1 togoal: 3.6950952627794e+16
Mobile Platform 2 Pose: [-2.34491612010879;9.97434339289117;6.41499457681346]
Robot Arm 2 end: [3.99983312941393e+16;-1.57518993807037e+15;-1.03410639657964e+16]
Robot Arm 1 togoal: 4.13435041282354e+16
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.673361e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.746152e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.785395759505415;-9.97305746965453;-6.65109586276226]
Robot Arm 1 end: [-1.15380471164721e+16;-355792795435052;-5.83124597836399e+16]
Robot Arm 1 togoal: 5.94440584566182e+16
Mobile Platform 2 Pose: [-2.34491612010879;9.97248531887693;6.41194115031452]
Robot Arm 2 end: [-5.24700671287985e+16;2.26286887080288e+15;4.84476700293958e+16]
Robot Arm 1 togoal: 7.14521185921334e+16
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183064e-34。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.588710e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.785395759505415;-9.97285991725431;-6.65233929974818]
Robot Arm 1 end: [-3.15127318345468e+16;-34800493410747.2;9.14559981044358e+16]
Robot Arm 1 togoal: 9.6732895480436e+16
Mobile Platform 2 Pose: [-2.34491612010879;9.97386268050381;6.41428117302368]
Robot Arm 2 end: [2.53831409872456e+16;-1.42236298573047e+15;-1.11979559474636e+17]
Robot Arm 1 togoal: 1.14829215372113e+17
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.344629e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.362833e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.785395759505415;-9.97206146908278;-6.65624996197738]
Robot Arm 1 end: [1.17279267160194e+17;1.05835410343865e+15;-1.15018912957547e+17]
Robot Arm 1 togoal: 1.64270803726579e+17
Mobile Platform 2 Pose: [-2.34491612010879;9.97350028311556;6.41346842016608]
Robot Arm 2 end: [6.46351585329518e+16;-1.83614873013158e+15;1.76980422593399e+17]
Robot Arm 1 togoal: 1.88422782969781e+17
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.852530e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.671961e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.785395759505415;-9.97356419826383;-6.64863342522325]
Robot Arm 1 end: [-2.51413565718648e+17;-2.68327449181269e+15;4.88994729821429e+16]
Robot Arm 1 togoal: 2.56138906547394e+17
Mobile Platform 2 Pose: [-2.34491612010879;9.97277185566454;6.41250502607751]
Robot Arm 2 end: [-2.44408818499585e+17;8.89193204659457e+15;-2.0822469558346e+17]
Robot Arm 1 togoal: 3.21204391107452e+17
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.176508e-35。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.796647e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163721579333;-6.65853078357501]
Robot Arm 1 end: [4.31429105864263e+17;5.87873991411116e+15;1.04356005122606e+17]
Robot Arm 1 togoal: 4.43909685380894e+17
Mobile Platform 2 Pose: [-2.34491612010879;9.97429051848669;6.41482036363134]
Robot Arm 2 end: [5.01264900756124e+17;-1.91128323653128e+16;3.44565677579269e+16]
Robot Arm 1 togoal: 5.02811153568344e+17
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.271319e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.465868e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.785395759505415;-9.97346885736037;-6.64904234075487]
Robot Arm 1 end: [-4.78474954606557e+17;-7.4277452746376e+15;-5.0593306352166e+17]
Robot Arm 1 togoal: 6.9639192869391e+17
Mobile Platform 2 Pose: [-2.34491612010879;9.97235377029124;6.41172158210947]
Robot Arm 2 end: [-8.00002339534551e+17;3.31470956800263e+16;3.51044227041205e+17]
Robot Arm 1 togoal: 8.74262273320617e+17
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.031279e-36。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.232872e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.785395759505415;-9.97227493068446;-6.65538143162157]
Robot Arm 1 end: [3.20038604145968e+17;8.02147610254615e+15;1.12522620021454e+18]
Robot Arm 1 togoal: 1.16988164096708e+18
Mobile Platform 2 Pose: [-2.34491612010879;9.97410012949022;6.41460169167248]
Robot Arm 2 end: [7.0676181787356e+17;-3.19402196765616e+16;-1.19065976827626e+18]
Robot Arm 1 togoal: 1.38499210417555e+18
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.041627e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.756412e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.785395759505415;-9.97262916365023;-6.65336195104755]
Robot Arm 1 end: [4.42680528368456e+17;-1.56787016336237e+15;-1.87703196798072e+18]
Robot Arm 1 togoal: 1.92852729232309e+18
Mobile Platform 2 Pose: [-2.34491612010879;9.97316932812719;6.41289348217991]
Robot Arm 2 end: [2.549547913292e+16;7.82592919051058e+15;2.31361397537106e+18]
Robot Arm 1 togoal: 2.31376768316445e+18
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.761691e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.760911e-37。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.785395759505415;-9.97324708416375;-6.65038084070059]
Robot Arm 1 end: [-2.17833341707734e+18;-1.72044307240018e+16;2.1594176303196e+18]
Robot Arm 1 togoal: 3.06733059361516e+18
Mobile Platform 2 Pose: [-2.34491612010879;9.97308027144829;6.41295961509572]
Robot Arm 2 end: [-2.03125461422247e+18;6.73258600058818e+16;-3.30909538590085e+18]
Robot Arm 1 togoal: 3.88338001645678e+18
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.312919e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.431898e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.785395759505415;-9.97175093531609;-6.65793527848102]
Robot Arm 1 end: [4.98030810458604e+18;5.72027161924135e+16;-1.7760258397521e+18]
Robot Arm 1 togoal: 5.2878170118502e+18
Mobile Platform 2 Pose: [-2.34491612010879;9.97415173440533;6.41450698146359]
Robot Arm 2 end: [5.65761718916474e+18;-2.08581758735554e+17;2.35639982695177e+18]
Robot Arm 1 togoal: 6.13227190800179e+18
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.280231e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.538191e-38。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362749625136;-6.64831780135496]
Robot Arm 1 end: [-7.96343552793198e+18;-9.81848849503355e+16;-2.00042672329771e+18]
Robot Arm 1 togoal: 8.21143426902058e+18
Mobile Platform 2 Pose: [-2.34491612010879;9.97233765515017;6.41168342223291]
Robot Arm 2 end: [-1.06221638099946e+19;4.26718889620396e+17;9.17317839663656e+17]
Robot Arm 1 togoal: 1.06702354723822e+19
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.480564e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.025999e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.785395759505415;-9.97180145232356;-6.65780304278897]
Robot Arm 1 end: [1.09027911256905e+19;1.69575223806546e+17;8.97423858195061e+18]
Robot Arm 1 togoal: 1.41222012523805e+19
Mobile Platform 2 Pose: [-2.34491612010879;9.97426128103619;6.41479139154717]
Robot Arm 2 end: [1.25650080430053e+19;-5.26423399722296e+17;-1.10399523672199e+19]
Robot Arm 1 togoal: 1.67343089784744e+19
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.497092e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.751525e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.785395759505415;-9.97317561661089;-6.65059885830373]
Robot Arm 1 end: [-7.30259225190732e+18;-1.66268475020355e+17;-2.1343919653761e+19]
Robot Arm 1 togoal: 2.25592199552558e+19
Mobile Platform 2 Pose: [-2.34491612010879;9.97285073835203;6.41235586800362]
Robot Arm 2 end: [-9.16541109730778e+18;4.62729176928203e+17;2.68936769406536e+19]
Robot Arm 1 togoal: 2.84163463214763e+19
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.274507e-39。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.726469e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.785395759505415;-9.97272188372313;-6.65316059382717]
Robot Arm 1 end: [-6.85934333249977e+18;5.22651782775144e+16;3.63564671805158e+19]
Robot Arm 1 togoal: 3.69979192448696e+19
Mobile Platform 2 Pose: [-2.34491612010879;9.97341178959596;6.41344533562425]
Robot Arm 2 end: [-1.10264248668124e+19;2.55165264653243e+17;-4.55406685708338e+19]
Robot Arm 1 togoal: 4.68572262146862e+19
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.044593e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.044757e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.785395759505415;-9.97218739585931;-6.65570603079299]
Robot Arm 1 end: [3.82704888204913e+19;2.98441251743119e+17;-4.93264748947674e+19]
Robot Arm 1 togoal: 6.24325276381168e+19
Mobile Platform 2 Pose: [-2.34491612010879;9.97393366778392;6.41407249235708]
Robot Arm 2 end: [5.6088828883272e+19;-1.97427274333426e+18;4.97270890862376e+19]
Robot Arm 1 togoal: 7.49842507954347e+19
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.287959e-40。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.778644e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.785395759505415;-9.97351166833506;-6.64903045036329]
Robot Arm 1 end: [-9.23501000709724e+19;-9.36576845304598e+17;3.17969101535217e+19]
Robot Arm 1 togoal: 9.76752868161487e+19
Mobile Platform 2 Pose: [-2.34491612010879;9.9724390640789;6.41182628422622]
Robot Arm 2 end: [-1.26159874986147e+20;4.90945180982359e+18;-3.04216435265864e+19]
Robot Arm 1 togoal: 1.29868753626313e+20
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.768295e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.036705e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.785395759505415;-9.97162998424949;-6.65866108782289]
Robot Arm 1 end: [1.68520906926549e+20;2.2136451828702e+18;1.64397347188264e+19]
Robot Arm 1 togoal: 1.69335351223446e+20
Mobile Platform 2 Pose: [-2.34491612010879;9.97433774845769;6.41483574876956]
Robot Arm 2 end: [1.86215005521409e+20;-7.45896510209952e+18;-7.96843337886409e+19]
Robot Arm 1 togoal: 2.02685119071609e+20
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.691429e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.356033e-41。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.785395759505415;-9.97353142050743;-6.6488373115566]
Robot Arm 1 end: [-2.06024072018945e+20;-2.99148446853382e+18;-1.65996606826972e+20]
Robot Arm 1 togoal: 2.64593538675194e+20
Mobile Platform 2 Pose: [-2.34491612010879;9.97258378519526;6.41191344391668]
Robot Arm 2 end: [-2.15375432658227e+20;9.55536580397158e+18;2.74035275016378e+20]
Robot Arm 1 togoal: 3.48673506251433e+20
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.125952e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.112585e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.785395759505415;-9.9721430806182;-6.65615720124416]
Robot Arm 1 end: [1.83572724739062e+20;3.74689062951731e+18;4.08097842520071e+20]
Robot Arm 1 togoal: 4.47500651985056e+20
Mobile Platform 2 Pose: [-2.34491612010879;9.97372777178332;6.41390097323901]
Robot Arm 2 end: [4.21425988685147e+19;-3.83665156276789e+18;-5.63204106601124e+20]
Robot Arm 1 togoal: 5.64791629034086e+20
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.954748e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.794998e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.785395759505415;-9.97276979214773;-6.65274644053131]
Robot Arm 1 end: [7.10769095259097e+19;-1.855710806614e+18;-7.28488120379633e+20]
Robot Arm 1 togoal: 7.31949665116805e+20
Mobile Platform 2 Pose: [-2.34491612010879;9.97365066886479;6.41354723551859]
Robot Arm 2 end: [4.4888160219898e+20;-1.43385272398281e+19;8.01701507642559e+20]
Robot Arm 1 togoal: 9.18926326487865e+20
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.454244e-42。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.255816e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.785395759505415;-9.97313630914675;-6.65107205591915]
Robot Arm 1 end: [-7.06034480580284e+20;-4.75498425915017e+18;9.35439905354987e+20]
Robot Arm 1 togoal: 1.17198767663066e+21
Mobile Platform 2 Pose: [-2.34491612010879;9.97264486837573;6.4121278039316]
Robot Arm 2 end: [-1.33857437435002e+21;5.00019470294558e+19;-8.30696606896158e+20]
Robot Arm 1 togoal: 1.57617841727454e+21
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.664896e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.092537e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.785395759505415;-9.9718309540204;-6.65762292500447]
Robot Arm 1 end: [1.78579999000912e+21;1.93832117733513e+19;-9.28292028522908e+20]
Robot Arm 1 togoal: 2.01275517722211e+21
Mobile Platform 2 Pose: [-2.34491612010879;9.97432654514784;6.41473110548082]
Robot Arm 2 end: [2.45106370045613e+21;-9.48744897091167e+19;-2.07756031410963e+20]
Robot Arm 1 togoal: 2.46168174244334e+21
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.393394e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.325081e-43。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362480180752;-6.64845585210596]
Robot Arm 1 end: [-3.10735489528217e+21;-3.66327867115177e+19;-3.40245083208925e+20]
Robot Arm 1 togoal: 3.12614189104464e+21
Mobile Platform 2 Pose: [-2.34491612010879;9.97240255171218;6.41161398546428]
Robot Arm 2 end: [-3.5919544809796e+21;1.5137172212758e+20;2.30521991697772e+21]
Robot Arm 1 togoal: 4.27072467589753e+21
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.770711e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.781127e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.785395759505415;-9.97172783314565;-6.65827327764423]
Robot Arm 1 end: [4.61105473826769e+21;6.84146444813473e+19;2.80012197258799e+21]
Robot Arm 1 togoal: 5.39510791590404e+21
Mobile Platform 2 Pose: [-2.34491612010879;9.97399613159363;6.4142744119821]
Robot Arm 2 end: [2.5697849984624e+21;-1.24077477401217e+20;-6.30394906985586e+21]
Robot Arm 1 togoal: 6.80874173647798e+21
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.770570e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.271097e-44。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.785395759505415;-9.97328232426335;-6.65016186088909]
Robot Arm 1 end: [-3.83217348912555e+21;-7.36388253893842e+19;-7.65681831170086e+21]
Robot Arm 1 togoal: 8.56258389656761e+21
Mobile Platform 2 Pose: [-2.34491612010879;9.97332733204716;6.41297542120414]
Robot Arm 2 end: [1.93261025303661e+21;-3.29261386426044e+19;1.11122575906431e+22]
Robot Arm 1 togoal: 1.12791105713861e+22
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.800077e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.718687e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.785395759505415;-9.97258045576842;-6.65399576314064]
Robot Arm 1 end: [-5.77068010969094e+20;4.50565193970269e+19;1.41411201800982e+22]
Robot Arm 1 togoal: 1.41529614401793e+22
Mobile Platform 2 Pose: [-2.34491612010879;9.97292854371594;6.41254605833698]
Robot Arm 2 end: [-1.22820475946502e+22;4.29510157123048e+20;-1.45925165319825e+22]
Robot Arm 1 togoal: 1.90781212604519e+22
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.252123e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.357779e-45。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.785395759505415;-9.97232185839645;-6.65511934693947]
Robot Arm 1 end: [1.18857499833626e+22;7.21954051829137e+19;-2.05245190477298e+22]
Robot Arm 1 togoal: 2.37177601595125e+22
Mobile Platform 2 Pose: [-2.34491612010879;9.97422806569169;6.4144821800786]
Robot Arm 2 end: [2.91092431558465e+22;-1.09121072671235e+21;7.10193286176635e+21]
Robot Arm 1 togoal: 2.99829322837064e+22
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.299201e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.555762e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.785395759505415;-9.97344401124045;-6.64950499691279]
Robot Arm 1 end: [-3.31905033719701e+22;-3.17360018465533e+20;1.69414977736878e+22]
Robot Arm 1 togoal: 3.72655951016745e+22
Mobile Platform 2 Pose: [-2.34491612010879;9.97233043178955;6.41148838813969]
Robot Arm 2 end: [-5.06405363840739e+22;2.06366914405072e+21;1.24440775085254e+22]
Robot Arm 1 togoal: 5.21879078009553e+22
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.090327e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.380418e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164480226432;-6.65867921538385]
Robot Arm 1 end: [6.44995749547201e+22;8.15769090247243e+20;-2.70674666160685e+21]
Robot Arm 1 togoal: 6.45614987902099e+22
Mobile Platform 2 Pose: [-2.34491612010879;9.9741949976522;6.41452883094043]
Robot Arm 2 end: [5.29593346732068e+22;-2.28567269336292e+21;-6.28107425554173e+22]
Robot Arm 1 togoal: 8.21894446328287e+22
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.166395e-46。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.339905e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357870156307;-6.64871271831634]
Robot Arm 1 end: [-8.61116412046667e+22;-1.18081009638643e+21;-5.19481477471627e+22]
Robot Arm 1 togoal: 1.00574445650006e+23
Mobile Platform 2 Pose: [-2.34491612010879;9.9729979242482;6.41241260319815]
Robot Arm 2 end: [-2.30801563438074e+22;1.42531943950189e+21;1.36588798615556e+23]
Robot Arm 1 togoal: 1.38532397148105e+23
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.104760e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.986405e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.785395759505415;-9.97202085666691;-6.65688112553901]
Robot Arm 1 end: [9.20424345649964e+22;1.66567489043215e+21;1.44272922629247e+23]
Robot Arm 1 togoal: 1.71141054213993e+23
Mobile Platform 2 Pose: [-2.34491612010879;9.97325407143076;6.41302481574632]
Robot Arm 2 end: [-8.67318961550274e+22;2.58220907302581e+21;-2.13428725202683e+23]
Robot Arm 1 togoal: 2.30392947713218e+23
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.823265e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.083872e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.785395759505415;-9.97290602225572;-6.65215437441998]
Robot Arm 1 end: [-1.02748216151166e+22;-1.16136794235913e+21;-2.77569854735581e+23]
Robot Arm 1 togoal: 2.77762389449421e+23
Mobile Platform 2 Pose: [-2.34491612010879;9.97404640288783;6.4141022267196]
Robot Arm 2 end: [3.08587287972908e+23;-1.11343767490987e+22;1.96826404853146e+23]
Robot Arm 1 togoal: 3.66184000594619e+23
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.180253e-47。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.856754e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.785395759505415;-9.97301470348317;-6.65181617627656]
Robot Arm 1 end: [-2.1576798567024e+23;-1.07643452424076e+21;3.92577488133028e+23]
Robot Arm 1 togoal: 4.47966590875163e+23
Mobile Platform 2 Pose: [-2.34491612010879;9.97237676937566;6.41154677706631]
Robot Arm 2 end: [-6.33300273217742e+23;2.50343738030934e+22;-5.3277581839674e+22]
Robot Arm 1 togoal: 6.36030232501618e+23
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.634713e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.106470e-48。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.785395759505415;-9.97192811744226;-6.65722394050468]
Robot Arm 1 end: [6.25146580738675e+23;6.322772407066e+21;-4.4216168075765e+23]
Robot Arm 1 togoal: 7.65738321354364e+23
Mobile Platform 2 Pose: [-2.34491612010879;9.97431229990419;6.41464322342239]
Robot Arm 2 end: [8.44537542568639e+23;-3.44864740371579e+22;-5.23576283463791e+23]
Robot Arm 1 togoal: 9.94266112419179e+23
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.897878e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.167329e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360629777569;-6.64867653087839]
Robot Arm 1 end: [-1.19017819126577e+24;-1.34186062848509e+22;3.41128887609742e+22]
Robot Arm 1 togoal: 1.19074257299339e+24
Mobile Platform 2 Pose: [-2.34491612010879;9.97270174102579;6.41191822433028]
Robot Arm 2 end: [-8.24355020936236e+23;3.81622511579109e+22;1.48713223057766e+24]
Robot Arm 1 togoal: 1.70075860402913e+24
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.379776e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.860197e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.785395759505415;-9.97167388818692;-6.65864231895219]
Robot Arm 1 end: [1.89438241676491e+24;2.69809249792894e+22;8.09229625096413e+23]
Robot Arm 1 togoal: 2.06016147362154e+24
Mobile Platform 2 Pose: [-2.34491612010879;9.97358195837146;6.41350198974553]
Robot Arm 2 end: [-2.04043060537795e+23;-2.81886737398956e+21;-2.77051145681517e+24]
Robot Arm 1 togoal: 2.77801642344156e+24
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.310468e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.095679e-49。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.785395759505415;-9.97337617242628;-6.64979235819222]
Robot Arm 1 end: [-1.83156036274423e+24;-3.13950383329423e+22;-2.68550537147502e+24]
Robot Arm 1 togoal: 3.25077500160021e+24
Mobile Platform 2 Pose: [-2.34491612010879;9.9737918298634;6.41361540784584]
Robot Arm 2 end: [2.78320877115526e+24;-9.42171575452134e+22;3.51327479086617e+24]
Robot Arm 1 togoal: 4.48310469348499e+24
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.975095e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.473319e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.785395759505415;-9.97243861294199;-6.65482956770248]
Robot Arm 1 end: [5.67809601415966e+23;2.66135361363298e+22;5.38443590639886e+24]
Robot Arm 1 togoal: 5.41435739990289e+24
Mobile Platform 2 Pose: [-2.34491612010879;9.97253556134326;6.41177708431532]
Robot Arm 2 end: [-7.10147584070151e+24;2.70960293401136e+23;-3.0402132847618e+24]
Robot Arm 1 togoal: 7.72963617601174e+24
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.050431e-50。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.594855e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.785395759505415;-9.9724616911209;-6.65450605335018]
Robot Arm 1 end: [3.42632431770506e+24;1.14581802643836e+22;-8.32961566143962e+24]
Robot Arm 1 togoal: 9.00679336319493e+24
Mobile Platform 2 Pose: [-2.34491612010879;9.97434285476393;6.4146092153242]
Robot Arm 2 end: [1.17143914384487e+25;-4.60538752954697e+23;-2.82752835228166e+24]
Robot Arm 1 togoal: 1.20596011252085e+25
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.981206e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.483916e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.785395759505415;-9.97336178709026;-6.65005354797757]
Robot Arm 1 end: [-1.1632577479159e+25;-1.03447593859047e+23;8.18597609414272e+24]
Robot Arm 1 togoal: 1.42245479656539e+25
Mobile Platform 2 Pose: [-2.34491612010879;9.9724761401798;6.4115458886509]
Robot Arm 2 end: [-1.5547955219059e+25;6.68850780977117e+23;1.38772055597443e+25]
Robot Arm 1 togoal: 2.0850973766433e+25
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.582838e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.126832e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.785395759505415;-9.97168130140373;-6.65858726792825]
Robot Arm 1 end: [2.42014758968432e+25;2.93806854490961e+23;-4.41104585657355e+24]
Robot Arm 1 togoal: 2.46019325176301e+25
Mobile Platform 2 Pose: [-2.34491612010879;9.97387631119008;6.41392005033325]
Robot Arm 2 end: [7.91543011636723e+24;-4.34139729218218e+23;-3.25301047514163e+25]
Robot Arm 1 togoal: 3.34820881423142e+25
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.296617e-51。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.178670e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361040725388;-6.64867010264281]
Robot Arm 1 end: [-3.51197224630428e+25;-4.57633868674874e+23;-1.51394872634907e+25]
Robot Arm 1 togoal: 3.8246678407939e+25
Mobile Platform 2 Pose: [-2.34491612010879;9.97348390651047;6.41305935280382]
Robot Arm 2 end: [1.79784798975546e+25;-5.00477614719957e+23;5.19663878676479e+25]
Robot Arm 1 togoal: 5.49907418142403e+25
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.797172e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.994564e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.785395759505415;-9.97191119270967;-6.657538662821]
Robot Arm 1 end: [4.27695176205648e+25;7.13494903700383e+23;4.95138274411939e+25]
Robot Arm 1 togoal: 6.54321008401055e+25
Mobile Platform 2 Pose: [-2.34491612010879;9.97278628688593;6.4121462025267]
Robot Arm 2 end: [-7.0303289306468e+25;2.54947100878135e+24;-6.18562151396736e+25]
Robot Arm 1 togoal: 9.36762704271425e+25
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.178170e-52。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.625290e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.785395759505415;-9.97303524657683;-6.65159921800714]
Robot Arm 1 end: [-1.79586402900975e+25;-6.03066678554575e+23;-1.03857559966551e+26]
Robot Arm 1 togoal: 1.05400518085509e+26
Mobile Platform 2 Pose: [-2.34491612010879;9.97428556642346;6.41442770051162]
Robot Arm 2 end: [1.46051930512517e+26;-5.5599246729463e+24;1.2441819627859e+25]
Robot Arm 1 togoal: 1.46686325349209e+26
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.204101e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.958019e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.785395759505415;-9.9728842116528;-6.65260279397473]
Robot Arm 1 end: [-5.99986952240636e+25;-1.15700343122586e+23;1.60426399440952e+26]
Robot Arm 1 togoal: 1.71278972593683e+26
Mobile Platform 2 Pose: [-2.34491612010879;9.97235013685442;6.41133515116621]
Robot Arm 2 end: [-2.35095383455003e+26;9.72464732441647e+24;9.85286922054303e+25]
Robot Arm 1 togoal: 2.55092750338287e+26
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.979716e-53。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.600678e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.785395759505415;-9.97204005718098;-6.65675040958764]
Robot Arm 1 end: [2.127821576771e+26;1.95979909724187e+24;-1.98751246818657e+26]
Robot Arm 1 togoal: 2.91173737741223e+26
Mobile Platform 2 Pose: [-2.34491612010879;9.97411022164842;6.41423451168241]
Robot Arm 2 end: [2.11555058442026e+26;-9.5099056568891e+24;-3.43887742452317e+26]
Robot Arm 1 togoal: 4.03862303844965e+26
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.914875e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.213794e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357207933221;-6.64897894448911]
Robot Arm 1 end: [-4.47435181636028e+26;-4.81677001286929e+24;7.54776091055551e+25]
Robot Arm 1 togoal: 4.53782230277159e+26
Mobile Platform 2 Pose: [-2.34491612010879;9.97315264506286;6.41248487784364]
Robot Arm 2 end: [-4.13601855837843e+24;2.73648792436586e+24;6.75266409482541e+26]
Robot Arm 1 togoal: 6.7528462057944e+26
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.893738e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.109196e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164092256793;-6.65890392842791]
Robot Arm 1 end: [7.59283570146613e+26;1.04146762772182e+25;2.04106902495173e+26]
Robot Arm 1 togoal: 7.8630759440738e+26
Mobile Platform 2 Pose: [-2.34491612010879;9.97309681269419;6.41260384151059]
Robot Arm 2 end: [-5.76983642286222e+26;1.89836081429088e+25;-9.74209543841595e+26]
Robot Arm 1 togoal: 1.13241102792054e+27
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.015071e-54。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.066781e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.785395759505415;-9.97345609512912;-6.64949601796675]
Robot Arm 1 end: [-8.26214005587933e+26;-1.29992186321624e+25;-9.17205893372535e+26]
Robot Arm 1 togoal: 1.23453036153511e+27
Mobile Platform 2 Pose: [-2.34491612010879;9.97414257535688;6.41410769001422]
Robot Arm 2 end: [1.63912083107682e+27;-6.03157477816185e+25;7.14798313367782e+26]
Robot Arm 1 togoal: 1.78921539147579e+27
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.295636e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.782403e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.785395759505415;-9.97229949851808;-6.65564607699038]
Robot Arm 1 end: [5.14200317459579e+26;1.36082718925603e+25;2.0063851253377e+27]
Robot Arm 1 togoal: 2.07127217494848e+27
Mobile Platform 2 Pose: [-2.34491612010879;9.97234003358461;6.41130632383994]
Robot Arm 2 end: [-3.1028688305529e+27;1.24460659128957e+26;2.16792954069697e+26]
Robot Arm 1 togoal: 3.11292219951316e+27
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.663433e-55。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.477017e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.785395759505415;-9.97260369306175;-6.65388250254672]
Robot Arm 1 end: [8.65103262709102e+26;-1.70106921526264e+24;-3.3079727122552e+27]
Robot Arm 1 togoal: 3.4192235981011e+27
Mobile Platform 2 Pose: [-2.34491612010879;9.97426723738647;6.41441711034079]
Robot Arm 2 end: [3.71501619246262e+27;-1.55219565470339e+26;-3.16085059034946e+27]
Robot Arm 1 togoal: 4.88020643807993e+27
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.460918e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.062525e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.785395759505415;-9.97326581354182;-6.65067125457295]
Robot Arm 1 end: [-3.95463641020046e+27;-3.19144105394597e+25;3.7238741113522e+27]
Robot Arm 1 togoal: 5.43207198623899e+27
Mobile Platform 2 Pose: [-2.34491612010879;9.97283574679083;6.41195074597166]
Robot Arm 2 end: [-2.80860471567253e+27;1.40153533979616e+26;7.80188145715239e+27]
Robot Arm 1 togoal: 8.29320551617065e+27
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.699291e-56。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.913737e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.785395759505415;-9.97173858609462;-6.65838997009307]
Robot Arm 1 end: [8.89997834739105e+27;1.03158660625953e+26;-2.92817471087413e+27]
Robot Arm 1 togoal: 9.3698699793911e+27
Mobile Platform 2 Pose: [-2.34491612010879;9.97342843064917;6.41308945369839]
Robot Arm 2 end: [-3.00547721782531e+27;6.59451125014364e+25;-1.33277721401016e+28]
Robot Arm 1 togoal: 1.36626041545234e+28
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.460279e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.651063e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362637444889;-6.64871024553175]
Robot Arm 1 end: [-1.4020524056838e+28;-1.74290446252257e+26;-3.89112288969846e+27]
Robot Arm 1 togoal: 1.45515053974069e+28
Mobile Platform 2 Pose: [-2.34491612010879;9.97392081835576;6.41366779395544]
Robot Arm 2 end: [1.61211363932459e+28;-5.65699585098071e+26;1.47835727719852e+28]
Robot Arm 1 togoal: 2.18807010521784e+28
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.942754e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.028912e-57。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.785395759505415;-9.97181674037473;-6.65811675195156]
Robot Arm 1 end: [1.88899733156912e+28;2.96598055583249e+26;1.63850899244413e+28]
Robot Arm 1 togoal: 2.50078034642415e+28
Mobile Platform 2 Pose: [-2.34491612010879;9.97244714168952;6.41145789469603]
Robot Arm 2 end: [-3.66522957322721e+28;1.42379081255465e+27;-9.46743185300035e+27]
Robot Arm 1 togoal: 3.7882056816058e+28
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.949025e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.651603e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.785395759505415;-9.973155210024;-6.65109267909223]
Robot Arm 1 end: [-1.20281085076826e+28;-2.852508688153e+26;-3.81447805092402e+28]
Robot Arm 1 togoal: 3.99972629367168e+28
Mobile Platform 2 Pose: [-2.34491612010879;9.9743393196054;6.41445396143952]
Robot Arm 2 end: [5.46777878305675e+28;-2.18601350767938e+27;-2.2367216064042e+28]
Robot Arm 1 togoal: 5.91162540385963e+28
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.167521e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.613568e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.785395759505415;-9.97274715575063;-6.65341970113712]
Robot Arm 1 end: [-1.38275055155552e+28;7.15569611983497e+25;6.40280902884986e+28]
Robot Arm 1 togoal: 6.55042088355667e+28
Mobile Platform 2 Pose: [-2.34491612010879;9.97257237965669;6.41151451899253]
Robot Arm 2 end: [-6.41959597178231e+28;2.83814630953324e+27;7.88929267117742e+28]
Robot Arm 1 togoal: 1.01751020651962e+29
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.559897e-58。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.806677e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.785395759505415;-9.97216405879882;-6.65621616193646]
Robot Arm 1 end: [6.99462682646341e+28;5.6288683712286e+26;-8.57575393463447e+28]
Robot Arm 1 togoal: 1.10666855202809e+29
Mobile Platform 2 Pose: [-2.34491612010879;9.97374269165306;6.41354206494275]
Robot Arm 2 end: [1.48692107356842e+28;-1.21822279357252e+27;-1.63999573975361e+29]
Robot Arm 1 togoal: 1.64676767513748e+29
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.084885e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.196696e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.785395759505415;-9.9735223402095;-6.64936159462148]
Robot Arm 1 end: [-1.64954293251345e+29;-1.68912080227212e+27;5.21833947782634e+28]
Robot Arm 1 togoal: 1.7301987944083e+29
Mobile Platform 2 Pose: [-2.34491612010879;9.97363511679324;6.41313903384744]
Robot Arm 2 end: [1.2700728385712e+29;-4.0226763031407e+27;2.36153019009543e+29]
Robot Arm 1 togoal: 2.68170245300862e+29
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.218824e-59。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.416633e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.785395759505415;-9.97162972978945;-6.6590544235482]
Robot Arm 1 end: [2.97689574177384e+29;3.93642708411066e+27;3.66301305898719e+28]
Robot Arm 1 togoal: 2.99960571574219e+29
Mobile Platform 2 Pose: [-2.34491612010879;9.972657597345;6.41176643520409]
Robot Arm 2 end: [-3.86482894032396e+29;1.44006896731648e+28;-2.48481432244924e+29]
Robot Arm 1 togoal: 4.59694930810867e+29
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.269473e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.441781e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.785395759505415;-9.97352133270435;-6.64927691046041]
Robot Arm 1 end: [-3.5790552567119e+29;-5.25541897192644e+27;-3.03050728793983e+29]
Robot Arm 1 togoal: 4.69002909326909e+29
Mobile Platform 2 Pose: [-2.34491612010879;9.97432368372039;6.41434186972434]
Robot Arm 2 end: [7.15876049971677e+29;-2.76652208236811e+28;-4.90060397179804e+28]
Robot Arm 1 togoal: 7.18084587841234e+29
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.262930e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.598802e-60。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.785395759505415;-9.97216631030687;-6.65642921072278]
Robot Arm 1 end: [3.0594322684265e+29;6.42541215185887e+27;7.30798864077071e+29]
Robot Arm 1 togoal: 7.92281215042078e+29
Mobile Platform 2 Pose: [-2.34491612010879;9.97239619790277;6.41122325229657]
Robot Arm 2 end: [-1.05917984127505e+30;4.45496447234872e+28;6.55021837856481e+29]
Robot Arm 1 togoal: 1.24615416986716e+30
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.652938e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.288316e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.785395759505415;-9.97274474149442;-6.65326468773523]
Robot Arm 1 end: [1.57270958670479e+29;-2.89304529881539e+27;-1.2881422282516e+30]
Robot Arm 1 togoal: 1.29771064739303e+30
Mobile Platform 2 Pose: [-2.34491612010879;9.97400797449435;6.41391023440017]
Robot Arm 2 end: [7.78151090468219e+29;-3.72404231586123e+28;-1.82605940732502e+30]
Robot Arm 1 togoal: 1.98529567767477e+30
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.466839e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.474432e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.785395759505415;-9.97315722181556;-6.65135168758493]
Robot Arm 1 end: [-1.29267718533128e+30;-9.01820411743488e+27;1.62347065647811e+30]
Robot Arm 1 togoal: 2.07527169448412e+30
Mobile Platform 2 Pose: [-2.34491612010879;9.97331059829976;6.41256629136611]
Robot Arm 2 end: [5.08537216884066e+29;-7.41823803553233e+27;3.25221657641229e+30]
Robot Arm 1 togoal: 3.29174388297489e+30
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.163801e-61。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.878413e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.785395759505415;-9.97181525513105;-6.6580945658743]
Robot Arm 1 end: [3.20411119157299e+30;3.51645348710706e+28;-1.56648330055673e+30]
Robot Arm 1 togoal: 3.56671207183858e+30
Mobile Platform 2 Pose: [-2.34491612010879;9.9729442892129;6.41218915422578]
Robot Arm 2 end: [-3.51381410485076e+30;1.22309414726365e+29;-4.31157704613179e+30]
Robot Arm 1 togoal: 5.56341134387072e+30
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.520847e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.601076e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.785395759505415;-9.9736265338837;-6.64883334072206]
Robot Arm 1 end: [-5.48914677196601e+30;-6.52333166216258e+28;-7.39152765244809e+29]
Robot Arm 1 togoal: 5.53907343155436e+30
Mobile Platform 2 Pose: [-2.34491612010879;9.97422088200794;6.41408595627111]
Robot Arm 2 end: [8.45733906873238e+30;-3.16503371198016e+29;2.21114112746864e+30]
Robot Arm 1 togoal: 8.74733694292555e+30
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.389732e-62。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.701912e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.785395759505415;-9.97173978955795;-6.65860418870064]
Robot Arm 1 end: [8.03519830280549e+30;1.20169705947495e+29;5.16720273396887e+30]
Robot Arm 1 togoal: 9.55399584559106e+30
Mobile Platform 2 Pose: [-2.34491612010879;9.97232994816866;6.41110686333187]
Robot Arm 2 end: [-1.4832099416957e+31;6.03503226697093e+29;3.3868244833669e+30]
Robot Arm 1 togoal: 1.52258323036957e+31
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.594046e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.850557e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.785395759505415;-9.97326404150739;-6.65064453428679]
Robot Arm 1 end: [-6.46229159288319e+30;-1.27364753208128e+29;-1.3735333956597e+31]
Robot Arm 1 togoal: 1.5180146024036e+31
Mobile Platform 2 Pose: [-2.34491612010879;9.97420294468062;6.4141579734806]
Robot Arm 2 end: [1.57307606823853e+31;-6.76512781449527e+29;-1.8069184097002e+31]
Robot Arm 1 togoal: 2.39668503379416e+31
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.530664e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.603648e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.785395759505415;-9.97260619774154;-6.65425310013566]
Robot Arm 1 end: [-1.68057962381177e+30;7.17947629781954e+28;2.49992347806459e+31]
Robot Arm 1 togoal: 2.50557626500934e+31
Mobile Platform 2 Pose: [-2.34491612010879;9.97298192148535;6.4120055238851]
Robot Arm 2 end: [-7.41572845378808e+30;4.42240155508601e+29;3.97413606819518e+31]
Robot Arm 1 togoal: 4.04297459021009e+31
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.299823e-63。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.564010e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.785395759505415;-9.97229714486576;-6.65563636484882]
Robot Arm 1 end: [2.1949045495372e+31;1.41322666476629e+29;-3.58583268155207e+31]
Robot Arm 1 togoal: 4.20428373479082e+31
Mobile Platform 2 Pose: [-2.34491612010879;9.9732708669106;6.41266930237506]
Robot Arm 2 end: [-2.43068413698741e+31;7.12406388125907e+29;-6.26225619944201e+31]
Robot Arm 1 togoal: 6.7178235545354e+31
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.929383e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.291968e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.785395759505415;-9.9734574112126;-6.64982219978966]
Robot Arm 1 end: [-5.95288694586416e+31;-5.7574550412221e+29;2.85007840076104e+31]
Robot Arm 1 togoal: 6.60023671617767e+31
Mobile Platform 2 Pose: [-2.34491612010879;9.9740352448242;6.41369995413569]
Robot Arm 2 end: [8.90720833455233e+31;-3.20640624948443e+30;5.89215927243531e+31]
Robot Arm 1 togoal: 1.06845080194285e+32
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.329918e-64。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.496991e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164057362947;-6.65909276043566]
Robot Arm 1 end: [1.14343931088435e+32;1.45636244532417e+30;-1.92582595559252e+30]
Robot Arm 1 togoal: 1.14369420624309e+32
Mobile Platform 2 Pose: [-2.34491612010879;9.97238221422988;6.41117437215271]
Robot Arm 2 end: [-1.8446267047231e+32;7.28017493094113e+30;-1.85360673063824e+31]
Robot Arm 1 togoal: 1.85534534618189e+32
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.912020e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.408674e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357137402843;-6.6491377812373]
Robot Arm 1 end: [-1.50314655465137e+32;-2.08150214571453e+30;-9.57816949211001e+31]
Robot Arm 1 togoal: 1.78249716355298e+32
Mobile Platform 2 Pose: [-2.34491612010879;9.97431595926475;6.41426500534235]
Robot Arm 2 end: [2.48697893675816e+32;-1.01333479848613e+31;-1.48752644677462e+32]
Robot Arm 1 togoal: 2.89966681463076e+32
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.610568e-65。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.117263e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.785395759505415;-9.97204218700546;-6.65716329488246]
Robot Arm 1 end: [1.56204486202892e+32;2.87841714524559e+30;2.59611971798029e+32]
Robot Arm 1 togoal: 3.02995879008333e+32
Mobile Platform 2 Pose: [-2.34491612010879;9.97268845316203;6.41151610769325]
Robot Arm 2 end: [-2.47897173274195e+32;1.14124742918986e+31;4.29833528904665e+32]
Robot Arm 1 togoal: 4.96326621951147e+32
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.904992e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.161781e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.785395759505415;-9.97288189902315;-6.65266770272952]
Robot Arm 1 end: [-6.26066490042969e+30;-1.91581430893854e+30;-4.92401858369542e+32]
Robot Arm 1 togoal: 4.92445384174977e+32
Mobile Platform 2 Pose: [-2.34491612010879;9.97359785464816;6.41314477738547]
Robot Arm 2 end: [-4.68064365260612e+31;-1.32165874831267e+30;-8.08615625663765e+32]
Robot Arm 1 togoal: 8.0997025831182e+32
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.762917e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.322827e-66。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.785395759505415;-9.97303750116458;-6.65208664917367]
Robot Arm 1 end: [-4.00038992253988e+32;-2.14828012962507e+30;6.84858029794205e+32]
Robot Arm 1 togoal: 7.93137019312995e+32
Mobile Platform 2 Pose: [-2.34491612010879;9.97377744226697;6.41320865061459]
Robot Arm 2 end: [7.94783662145515e+32;-2.67766686966527e+31;1.03876115653714e+33]
Robot Arm 1 togoal: 1.30821359109677e+33
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.303745e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.916033e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.785395759505415;-9.97190943623266;-6.657710652033]
Robot Arm 1 end: [1.12679209613079e+33;1.15587413859598e+31;-7.56060692813076e+32]
Robot Arm 1 togoal: 1.35698997919005e+33
Mobile Platform 2 Pose: [-2.34491612010879;9.97254622556518;6.41141260372746]
Robot Arm 2 end: [-2.05679868901981e+33;7.83162437387912e+31;-9.19779039301175e+32]
Robot Arm 1 togoal: 2.25445065644045e+33
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.865422e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.113569e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.785395759505415;-9.97361089110066;-6.64903908603044]
Robot Arm 1 end: [-2.1093771246016e+33;-2.39763772971425e+31;7.73619598015379e+30]
Robot Arm 1 togoal: 2.10952757014197e+33
Mobile Platform 2 Pose: [-2.34491612010879;9.97434209659568;6.41422349129283]
Robot Arm 2 end: [3.4296298259097e+33;-1.34604364775419e+32;-7.69684359449504e+32]
Robot Arm 1 togoal: 3.51751234411014e+33
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.037726e-67。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.370771e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.785395759505415;-9.97168220530577;-6.65899192181497]
Robot Arm 1 end: [3.31694631788494e+33;4.75779487678876e+31;1.51866232695619e+33]
Robot Arm 1 togoal: 3.64838756168467e+33
Mobile Platform 2 Pose: [-2.34491612010879;9.97246726605804;6.41115110640205]
Robot Arm 2 end: [-4.60410353833527e+33;1.97581758134085e+32;3.97259055508877e+33]
Robot Arm 1 togoal: 6.08426525237347e+33
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.108093e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.325137e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.785395759505415;-9.97336025543699;-6.65026260462479]
Robot Arm 1 end: [-3.12976992969739e+33;-5.46264802050561e+31;-4.83826735795385e+33]
Robot Arm 1 togoal: 5.76257536976475e+33
Mobile Platform 2 Pose: [-2.34491612010879;9.97388971320847;6.41355854381039]
Robot Arm 2 end: [2.45641301978975e+33;-1.32173148137201e+32;-9.44718997420232e+33]
Robot Arm 1 togoal: 9.76221455785263e+33
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.602838e-68。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.125182e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.785395759505415;-9.97246427250723;-6.65508795187434]
Robot Arm 1 end: [7.5294136443952e+32;4.41496744108539e+31;9.5550657823696e+33]
Robot Arm 1 togoal: 9.58478753011485e+33
Mobile Platform 2 Pose: [-2.34491612010879;9.97346755760288;6.41265037760355]
Robot Arm 2 end: [4.9881297734268e+33;-1.35728733264503e+32;1.52524404502869e+34]
Robot Arm 1 togoal: 1.60479531596752e+34
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.927277e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.381206e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.785395759505415;-9.97243617299896;-6.65502703916857]
Robot Arm 1 end: [6.43194657462418e+33;2.54519305172305e+31;-1.46130951874015e+34]
Robot Arm 1 togoal: 1.59659993578743e+34
Mobile Platform 2 Pose: [-2.34491612010879;9.97280079313824;6.41178747473707]
Robot Arm 2 end: [-2.02137314353243e+34;7.30601155432394e+32;-1.8361521788618e+34]
Robot Arm 1 togoal: 2.73180196753284e+34
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.183485e-69。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.601392e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.785395759505415;-9.97337781149199;-6.65035746010124]
Robot Arm 1 end: [-2.09653407778529e+34;-1.89173561524076e+32;1.39629568577009e+34]
Robot Arm 1 togoal: 2.51901858821624e+34
Mobile Platform 2 Pose: [-2.34491612010879;9.97428042350272;6.41403473720912]
Robot Arm 2 end: [4.25412798420501e+34;-1.61688132476766e+33;4.32237103513195e+33]
Robot Arm 1 togoal: 4.27908598556204e+34
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.334820e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.525311e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.785395759505415;-9.97167318253503;-6.65902055845423]
Robot Arm 1 end: [4.30564730067372e+34;5.26771690000139e+32;-6.73927636541285e+33]
Robot Arm 1 togoal: 4.35838869551889e+34
Mobile Platform 2 Pose: [-2.34491612010879;9.97234681945307;6.41094920336263]
Robot Arm 2 end: [-6.90571120834512e+34;2.85184476381754e+33;2.76082700103299e+34]
Robot Arm 1 togoal: 7.44260325479367e+34
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.295728e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023419e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.785395759505415;-9.97360589781821;-6.64908033053431]
Robot Arm 1 end: [-6.15545192966429e+34;-8.09265766834896e+32;-2.83549139198855e+34]
Robot Arm 1 togoal: 6.77762119059945e+34
Mobile Platform 2 Pose: [-2.34491612010879;9.97412006331917;6.41386690987892]
Robot Arm 2 end: [6.32555617672283e+34;-2.82912482689184e+33;-9.92876222815969e+34]
Robot Arm 1 togoal: 1.17759509085707e+35
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.189726e-70。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.749636e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.785395759505415;-9.97193009921012;-6.65783358926821]
Robot Arm 1 end: [7.34016532602587e+34;1.24036053581154e+33;8.96138255765137e+34]
Robot Arm 1 togoal: 1.15844632720104e+35
Mobile Platform 2 Pose: [-2.34491612010879;9.97313608817613;6.4120765139004]
Robot Arm 2 end: [-4.56713783048976e+33;9.29784187736279e+32;1.97017655155564e+35]
Robot Arm 1 togoal: 1.97072777647232e+35
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.785655e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.474657e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.785395759505415;-9.97301250440459;-6.65210527922986]
Robot Arm 1 end: [-2.73448794479277e+34;-1.01858739920155e+33;-1.84841913763951e+35]
Robot Arm 1 togoal: 1.86856396829842e+35
Mobile Platform 2 Pose: [-2.34491612010879;9.97311331802427;6.41224799169677]
Robot Arm 2 end: [-1.63750984093388e+35;5.34591734822058e+33;-2.86680790799572e+35]
Robot Arm 1 togoal: 3.30195153564245e+35
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.383496e-71。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.822066e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.785395759505415;-9.97290852334082;-6.65286608318297]
Robot Arm 1 end: [-1.13649126179644e+35;-3.01088632606575e+32;2.81118987363464e+35]
Robot Arm 1 togoal: 3.03222854666367e+35
Mobile Platform 2 Pose: [-2.34491612010879;9.9741332652892;6.41370818102592]
Robot Arm 2 end: [4.74720420773371e+35;-1.74351586850664e+34;2.1640792829444e+35]
Robot Arm 1 togoal: 5.22011354365315e+35
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  6.188096e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.067056e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.785395759505415;-9.97201883463928;-6.65724994280447]
Robot Arm 1 end: [3.85642573811643e+35;3.62118316477395e+33;-3.42845378025515e+35]
Robot Arm 1 togoal: 5.16019632317453e+35
Mobile Platform 2 Pose: [-2.34491612010879;9.97234269854288;6.41092965376622]
Robot Arm 2 end: [-9.06076559943832e+35;3.62890350487345e+34;4.84900086887427e+34]
Robot Arm 1 togoal: 9.08098511994735e+35
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.891573e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.023657e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.785395759505415;-9.97357952620442;-6.64932668557674]
Robot Arm 1 end: [-7.95683023646412e+35;-8.63968858387415e+33;1.13686625720887e+35]
Robot Arm 1 togoal: 8.03810156197173e+35
Mobile Platform 2 Pose: [-2.34491612010879;9.97427294625946;6.41404241189209]
Robot Arm 2 end: [1.09778631111605e+36;-4.57458270437937e+34;-9.04478827883613e+35]
Robot Arm 1 togoal: 1.42313366050256e+36
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.292111e-72。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.430208e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.785395759505415;-9.97164538018495;-6.65927327461877]
Robot Arm 1 end: [1.33501050586597e+36;1.84346677638736e+34;3.95713438748066e+35]
Robot Arm 1 togoal: 1.39254515666599e+36
Mobile Platform 2 Pose: [-2.34491612010879;9.97282097267334;6.41154599279843]
Robot Arm 2 end: [-8.58187853051861e+35;4.23716667531379e+34;2.26252133654898e+36]
Robot Arm 1 togoal: 2.42018275086998e+36
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.155157e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.029329e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.785395759505415;-9.97344272661733;-6.6499528635802]
Robot Arm 1 end: [-1.4242160381582e+36;-2.27255926944431e+34;-1.66110173038213e+36]
Robot Arm 1 togoal: 2.18818800256944e+36
Mobile Platform 2 Pose: [-2.34491612010879;9.97344493663473;6.41273333986216]
Robot Arm 2 end: [-8.15291005094216e+35;1.67721584505848e+34;-3.89913255948973e+36]
Robot Arm 1 togoal: 3.98349286992704e+36
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.054811e-73。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.381168e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.785395759505415;-9.97232449675671;-6.65590843600349]
Robot Arm 1 end: [8.15868263350458e+35;2.30141844938941e+34;3.5745296692675e+36]
Robot Arm 1 togoal: 3.66652874423535e+36
Mobile Platform 2 Pose: [-2.34491612010879;9.97390787495354;6.41326298556479]
Robot Arm 2 end: [4.63124293753787e+36;-1.61996686876544e+35;4.39173698840233e+36]
Robot Arm 1 togoal: 6.38451312539606e+36
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.503166e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.206776e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.785395759505415;-9.97257794504912;-6.6544045168288]
Robot Arm 1 end: [1.67627592768751e+36;-1.07727204107686e+33;-5.82475489108286e+36]
Robot Arm 1 togoal: 6.06116091911882e+36
Mobile Platform 2 Pose: [-2.34491612010879;9.97245543923416;6.41108982614877]
Robot Arm 2 end: [-1.06449247745138e+37;4.12776574251233e+35;-2.93279874697748e+36]
Robot Arm 1 togoal: 1.10492586378263e+37
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.695488e-74。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.365298e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.785395759505415;-9.97328430658747;-6.65096279404567]
Robot Arm 1 end: [-7.17074596191778e+36;-5.90533165189148e+34;6.41015581436279e+36]
Robot Arm 1 togoal: 9.61837733242781e+36
Mobile Platform 2 Pose: [-2.34491612010879;9.97434066065996;6.41407179038275]
Robot Arm 2 end: [1.60487350651799e+37;-6.40435725299819e+35;-6.26809888191747e+36]
Robot Arm 1 togoal: 1.7241262097192e+37
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.444226e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.921229e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.785395759505415;-9.97172675634797;-6.65884206688231]
Robot Arm 1 end: [1.58921416643045e+37;1.85855917927507e+35;-4.7925892818249e+36]
Robot Arm 1 togoal: 1.66001090696662e+37
Mobile Platform 2 Pose: [-2.34491612010879;9.97256125799886;6.41111604914294]
Robot Arm 2 end: [-1.91172438835448e+37;8.42358840976879e+35;2.27023219107e+37]
Robot Arm 1 togoal: 2.96913118985496e+37
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.446382e-75。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.622075e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.785395759505415;-9.9736247208115;-6.64910546061936]
Robot Arm 1 end: [-2.46633017740002e+37;-3.09164705544725e+35;-7.5089767461095e+36]
Robot Arm 1 togoal: 2.57829162234253e+37
Mobile Platform 2 Pose: [-2.34491612010879;9.9737574053398;6.41318281021682]
Robot Arm 2 end: [5.08275425904201e+36;-3.83944256095281e+35;-4.77408432208695e+37]
Robot Arm 1 togoal: 4.80121850730545e+37
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  9.149030e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.660286e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.785395759505415;-9.97183275190953;-6.65842675468967]
Robot Arm 1 end: [3.26758623233523e+37;5.18182173833545e+35;2.98727365496495e+37]
Robot Arm 1 togoal: 4.42759627823576e+37
Mobile Platform 2 Pose: [-2.34491612010879;9.97361954906017;6.41273085513787]
Robot Arm 2 end: [3.58931784045391e+37;-1.12654797294265e+36;6.95293839163707e+37]
Robot Arm 1 togoal: 7.82555083946522e+37
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.534758e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.798945e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.785395759505415;-9.97313423901377;-6.65158944240027]
Robot Arm 1 end: [-1.96645308964862e+37;-4.88236573967868e+35;-6.81204561608396e+37]
Robot Arm 1 togoal: 7.09036578597458e+37
Mobile Platform 2 Pose: [-2.34491612010879;9.97267045160031;6.41140524001947]
Robot Arm 2 end: [-1.11547493192485e+38;4.14573424224418e+36;-7.42465694360263e+37]
Robot Arm 1 togoal: 1.34061864163354e+38
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.365706e-76。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.851330e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.785395759505415;-9.97277254049365;-6.65367811463745]
Robot Arm 1 end: [-2.74668794155646e+37;8.91984074240699e+34;1.1265445313095e+38]
Robot Arm 1 togoal: 1.15954573999556e+38
Mobile Platform 2 Pose: [-2.34491612010879;9.97432061776046;6.41395230384727]
Robot Arm 2 end: [2.09017025880877e+38;-8.06464429256526e+36;-1.09240906485384e+37]
Robot Arm 1 togoal: 2.09457612304137e+38
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  4.832783e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.930721e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.785395759505415;-9.97214079697954;-6.65672595884879]
Robot Arm 1 end: [1.27663719795505e+38;1.05767696264312e+36;-1.48890564551237e+38]
Robot Arm 1 togoal: 1.96131446343947e+38
Mobile Platform 2 Pose: [-2.34491612010879;9.97239015830573;6.4108330055082]
Robot Arm 2 end: [-3.12165607389577e+38;1.31051147270469e+37;1.85953958573476e+38]
Robot Arm 1 togoal: 3.63590408534189e+38
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.297545e-77。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.890615e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.785395759505415;-9.97353261239252;-6.64969476397232]
Robot Arm 1 end: [-2.94406005532068e+38;-3.0435200583588e+36;8.49667567305711e+37]
Robot Arm 1 togoal: 3.06436794228444e+38
Mobile Platform 2 Pose: [-2.34491612010879;9.97401957448111;6.41354564857131]
Robot Arm 2 end: [2.35193326417119e+38;-1.11631340861596e+37;-5.28788373166974e+38]
Robot Arm 1 togoal: 5.78841653606883e+38
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.290390e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.711607e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.785395759505415;-9.97163020081765;-6.65944409966805]
Robot Arm 1 end: [5.2540637006961e+38;6.99412701749089e+36;7.82746009313208e+37]
Robot Arm 1 togoal: 5.31251056162167e+38
Mobile Platform 2 Pose: [-2.34491612010879;9.97329394373247;6.41215733221832]
Robot Arm 2 end: [1.32254305513785e+38;-1.52778209731796e+36;9.51468543903953e+38]
Robot Arm 1 togoal: 9.60617470944485e+38
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  2.545690e-78。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  8.280601e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.785395759505415;-9.97351064518895;-6.64971964512201]
Robot Arm 1 end: [-6.20903967252363e+38;-9.22416358063402e+36;-5.52531852759139e+38]
Robot Arm 1 togoal: 8.3120290546709e+38
Mobile Platform 2 Pose: [-2.34491612010879;9.97296005064458;6.41183225447988]
Robot Arm 2 end: [-1.00469856628568e+39;3.48033256594479e+37;-1.27321464814787e+39]
Robot Arm 1 togoal: 1.62225337750666e+39
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.196894e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.889132e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.785395759505415;-9.97219006197891;-6.65669848244863]
Robot Arm 1 end: [5.07095318356795e+38;1.09946454506151e+37;1.30751382580314e+39]
Robot Arm 1 togoal: 1.40244748521801e+39
Mobile Platform 2 Pose: [-2.34491612010879;9.97421352780141;6.41368947463903]
Robot Arm 2 end: [2.45638970764714e+39;-9.17711482418669e+37;6.85296409205965e+38]
Robot Arm 1 togoal: 2.55184315896472e+39
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.160832e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.003123e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.785395759505415;-9.97271932060283;-6.6537848733473]
Robot Arm 1 end: [3.3456455155563e+38;-4.4124185370207e+36;-2.27593174533741e+39]
Robot Arm 1 togoal: 2.3003952308309e+39
Mobile Platform 2 Pose: [-2.34491612010879;9.97232976844808;6.41072579742291]
Robot Arm 2 end: [-4.34254581628527e+39;1.76426029870388e+38;9.17105947001146e+38]
Robot Arm 1 togoal: 4.44183674042365e+39
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.374476e-79。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.416088e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.785395759505415;-9.97317796542587;-6.65163208489244]
Robot Arm 1 end: [-2.36291483855304e+39;-1.70227746640837e+37;2.81349450239303e+39]
Robot Arm 1 togoal: 3.67415400114192e+39
Mobile Platform 2 Pose: [-2.34491612010879;9.97421064153789;6.41378669413238]
Robot Arm 2 end: [4.66920386879809e+39;-2.00115325841321e+38;-5.19589843649644e+39]
Robot Arm 1 togoal: 6.98848134249651e+39
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  3.701805e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.009127e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.785395759505415;-9.97179998546908;-6.6585640725155]
Robot Arm 1 end: [5.74416786475571e+39;6.37225233894161e+37;-2.63314528173002e+39]
Robot Arm 1 togoal: 6.31925463113937e+39
Mobile Platform 2 Pose: [-2.34491612010879;9.97296609446876;6.41159875553133]
Robot Arm 2 end: [-2.36078197436583e+39;1.36647239595069e+38;1.1558980305955e+40]
Robot Arm 1 togoal: 1.17983892846454e+40
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.469701e-80。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  5.231328e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.785395759505415;-9.97362775878573;-6.64921346205388]
Robot Arm 1 end: [-9.68873505607727e+39;-1.16080087776957e+38;-1.55220898966564e+39]
Robot Arm 1 togoal: 9.81297173751345e+39
Mobile Platform 2 Pose: [-2.34491612010879;9.97328757174787;6.41231362728448]
Robot Arm 2 end: [-6.80035866748359e+39;1.95919959619848e+38;-1.83672299019485e+40]
Robot Arm 1 togoal: 1.95866892788962e+40
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  7.362671e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
[警告: 矩阵接近奇异值，或者缩放错误。结果可能不准确。RCOND =  1.540994e-81。] 
[> 位置：<a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 12)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',12,0)">第 12 行</a>)
位置: <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 103)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',103,0)">第 103 行</a>)
] 

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
p1_G

p1_G =

   10.0000
   10.0000
    1.5210

pa1

pa1 =

   1.0e+39 *

   -9.6887
   -0.1161
   -1.5522

mobile_robot1.arm.joint_world{end}

ans =

   1.0e+40 *

    0.0000   -0.0000    0.0000    1.3984
    0.0000    0.0000    0.0000    0.0211
   -0.0000   -0.0000    0.0000    0.9516
         0         0         0    0.0000

mobile_robot1.arm.joint_base{end}

ans =

   1.0e+40 *

    0.0000    0.0000    0.0000    1.3985
   -0.0000    0.0000    0.0000    0.0019
   -0.0000   -0.0000    0.0000    0.9516
         0         0         0    0.0000

main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.560254e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785398045120017;-9.99859258844473;-9.82813556207301]
Robot Arm 1 end: [-9.8543202540212;-9.82617087977973;1.50722002620516]
Robot Arm 1 togoal: 28.0583548054967
Mobile Platform 2 Pose: [-2.35619437191491;9.99866771753545;9.81844390937036]
Robot Arm 2 end: [9.85353114962791;9.82440232449613;1.5072528659496]
Robot Arm 1 togoal: 28.0565467514523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.044533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397926814489;-9.99718189953243;-9.65601765681]
Robot Arm 1 end: [-9.71366847177073;-9.65218268358934;1.46633154211785]
Robot Arm 1 togoal: 27.835947935796
Mobile Platform 2 Pose: [-2.35619425360615;9.99733167719312;9.63690106041641]
Robot Arm 2 end: [9.71211394882368;9.64858382341401;1.46646789007876]
Robot Arm 1 togoal: 27.832305983302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.718945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397808565794;-9.99576828446292;-9.48363712722217]
Robot Arm 1 end: [-9.58292944168337;-9.47809089449275;1.39970337578914]
Robot Arm 1 togoal: 27.6206781828962
Mobile Platform 2 Pose: [-2.35619413523974;9.9959918816794;9.45537710504905]
Robot Arm 2 end: [9.58063369371247;9.47235163687273;1.40002006424032]
Robot Arm 1 togoal: 27.6150019058885

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.548690e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397690430117;-9.99435174049259;-9.3110197491575]
Robot Arm 1 end: [-9.46666013716577;-9.30397997824931;1.30958583733119]
Robot Arm 1 togoal: 27.4160390254454
Mobile Platform 2 Pose: [-2.35619401675617;9.99464806714803;9.27390316338631]
Robot Arm 2 end: [9.46364440583238;9.29555351089144;1.31016369290284]
Robot Arm 1 togoal: 27.4079603545531

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.508408e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397572438265;-9.99293194081251;-9.13822453354542]
Robot Arm 1 end: [-9.36893186004308;-9.12995967981924;1.19903473571644]
Robot Arm 1 togoal: 27.22525556137
Mobile Platform 2 Pose: [-2.35619389806692;9.99329972174126;9.09253508747493]
Robot Arm 2 end: [9.36521183654717;9.11808333107738;1.19995596721431]
Robot Arm 1 togoal: 27.2142537836214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.581021e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.78539745460183;-9.99150829037485;-8.96533994857816]
Robot Arm 1 end: [-9.29318828532975;-8.95615951135899;1.07180765269281]
Robot Arm 1 togoal: 27.051152866341
Mobile Platform 2 Pose: [-2.35619377906084;9.99194612037598;8.91135136619761]
Robot Arm 2 end: [9.28877275053306;8.93988457441786;1.07315289102519]
Robot Arm 1 togoal: 27.0365779814494

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397336921785;-9.99008000389273;-8.79247816206491]
Robot Arm 1 end: [-9.24212439269297;-8.78272159285594;0.932235984970961]
Robot Arm 1 togoal: 26.8960336228969
Mobile Platform 2 Pose: [-2.35619365961243;9.99058637435803;8.73044963872687]
Robot Arm 2 end: [9.23701534613405;8.76095145748847;0.934081083136806]
Robot Arm 1 togoal: 26.8771377351199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397219398917;-9.98864620107691;-8.61976755715354]
Robot Arm 1 end: [-9.21759059907089;-8.60979214754285;0.785077107000626]
Robot Arm 1 togoal: 26.7615719776079
Mobile Platform 2 Pose: [-2.35619353959171;9.98921949380744;8.54994188328513]
Robot Arm 2 end: [9.21178497521275;8.5813276888236;0.787489820492563]
Robot Arm 1 togoal: 26.7375477147156

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785397102045333;-9.98720601335702;-8.44734392693617]
Robot Arm 1 end: [-9.22052555872251;-8.43751209354812;0.63535169612284]
Robot Arm 1 togoal: 26.6487303238147
Mobile Platform 2 Pose: [-2.35619341887489;9.98784446038277;8.3699484611203]
Robot Arm 2 end: [9.21401846507646;8.40110288231507;0.638388374904755]
Robot Arm 1 togoal: 26.6187545941315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.78539698489615;-9.9857586957399;-8.27534089039409]
Robot Arm 1 end: [-9.23722458325564;-8.26581610165402;0.492766490622996]
Robot Arm 1 togoal: 26.547469025801
Mobile Platform 2 Pose: [-2.35619329735542;9.98646030734106;8.1905913108382]
Robot Arm 2 end: [9.23841315158389;8.22062537616628;0.493340624775725]
Robot Arm 1 togoal: 26.5172361584143

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396864116741;-9.98430112990173;-8.1060955166656]
Robot Arm 1 end: [-9.29239330281432;-8.09737356344165;0.354468862780728]
Robot Arm 1 togoal: 26.4777673556388
Mobile Platform 2 Pose: [-2.35605401733851;9.98506726203874;8.01267505456271]
Robot Arm 2 end: [9.29292294066505;8.0403803781154;0.355143095903166]
Robot Arm 1 togoal: 26.4392023726579

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396742706797;-9.98283345639928;-7.93788133647499]
Robot Arm 1 end: [-9.32012149502887;-7.92957761246532;0.281373076078265]
Robot Arm 1 togoal: 26.3869953358583
Mobile Platform 2 Pose: [-2.35512331888079;9.98366359863651;7.83585611778094]
Robot Arm 2 end: [9.31968373707991;7.86237154981125;0.283965903457196]
Robot Arm 1 togoal: 26.3409329834823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396618808133;-9.98137224314818;-7.77225274842552]
Robot Arm 1 end: [-9.32874102087319;-7.76413605202121;0.260214410931695]
Robot Arm 1 togoal: 26.2822057565249
Mobile Platform 2 Pose: [-2.35463476096193;9.98226532559385;7.66148859730497]
Robot Arm 2 end: [9.32800020602594;7.68754946648183;0.263316394685724]
Robot Arm 1 togoal: 26.2298068262894

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.78539649445214;-9.97991265579281;-7.60780490752364]
Robot Arm 1 end: [-9.33038238324183;-7.59979116714435;0.254001766154922]
Robot Arm 1 togoal: 26.1729176198223
Mobile Platform 2 Pose: [-2.35424056968434;9.98086722502756;7.48834575458626]
Robot Arm 2 end: [9.32961914016226;7.51421741478666;0.257206072844197]
Robot Arm 1 togoal: 26.1147307634304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396369789931;-9.97844995869368;-7.44428163116747]
Robot Arm 1 end: [-9.32992355267945;-7.43634599161122;0.252060831997297]
Robot Arm 1 togoal: 26.0630449622392
Mobile Platform 2 Pose: [-2.35367661370629;9.97946475662816;7.31619660081771]
Robot Arm 2 end: [9.32922182988223;7.34195608976206;0.255254627017076]
Robot Arm 1 togoal: 25.9993147778409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396244829649;-9.97698244588208;-7.28162535031696]
Robot Arm 1 end: [-9.32886226608704;-7.27376092631995;0.25133775718859]
Robot Arm 1 togoal: 25.9538007862402
Mobile Platform 2 Pose: [-2.35281785901451;9.97805630595523;7.1449895444645]
Robot Arm 2 end: [9.32824436433893;7.17065587324496;0.254490229144573]
Robot Arm 1 togoal: 25.8846769407052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785396119572724;-9.97550958532219;-7.11982465882456]
Robot Arm 1 end: [-9.32762845380751;-7.11202964522685;0.250961090311645]
Robot Arm 1 togoal: 25.8455369272854
Mobile Platform 2 Pose: [-2.35156767909319;9.97664137750105;6.97471575612891]
Robot Arm 2 end: [9.32709934225449;7.00029066150066;0.254064017160211]
Robot Arm 1 togoal: 25.7711423562382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785395994021568;-9.97403122188229;-6.95888063009814]
Robot Arm 1 end: [-9.32634274178096;-6.95115462149655;0.250682896716331]
Robot Arm 1 togoal: 25.7383541105015
Mobile Platform 2 Pose: [-2.35141248528123;9.9752198198365;6.80537813814856]
Robot Arm 2 end: [9.39948332844295;6.82798605390175;0.165542016876725]
Robot Arm 1 togoal: 25.7168881162722

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395868179147;-9.97254731653599;-6.79879787112347]
Robot Arm 1 end: [-9.3250385847295;-6.79114084156113;0.250432886608803]
Robot Arm 1 togoal: 25.6322817527441
Mobile Platform 2 Pose: [-2.34939645667778;9.97378968846319;6.63731533721742]
Robot Arm 2 end: [9.36203709333619;6.66126059192067;0.201701306802018]
Robot Arm 1 togoal: 25.5778543657886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395742048652;-9.97105786605991;-6.63958196669843]
Robot Arm 1 end: [-9.323725169329;-6.63199399076049;0.250191225535183]
Robot Arm 1 togoal: 25.5273291487713
Mobile Platform 2 Pose: [-2.34925681793173;9.97236086694387;6.47016975805243]
Robot Arm 2 end: [9.43082169878561;6.4913113198247;0.129364855825794]
Robot Arm 1 togoal: 25.5236523505527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785395615633401;-9.96956287871977;-6.48123873001947]
Robot Arm 1 end: [-9.32240498492379;-6.47371990740119;0.249952361255873]
Robot Arm 1 togoal: 25.4234998394964
Mobile Platform 2 Pose: [-2.34689565903554;9.97092331995829;6.30427948618647]
Robot Arm 2 end: [9.38965581816233;6.32690040189785;0.162910056650912]
Robot Arm 1 togoal: 25.3844605570911

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785395488936804;-9.96806236694559;-6.32377397949578]
Robot Arm 1 end: [-9.32107868887628;-6.31632441481617;0.249714769321642]
Robot Arm 1 togoal: 25.3207956085313
Mobile Platform 2 Pose: [-2.34448601599226;9.96949225835018;6.13930758751857]
Robot Arm 2 end: [9.35716378198135;6.16304063706018;0.199324518971685]
Robot Arm 1 togoal: 25.2525344944792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395361962361;-9.96655634512881;-6.16719347024689]
Robot Arm 1 end: [-9.31974644283566;-6.15981326770371;0.249478056437761]
Robot Arm 1 togoal: 25.2192175833326
Mobile Platform 2 Pose: [-2.34448588923608;9.96804994839229;5.97511661406872]
Robot Arm 2 end: [9.32974474288869;5.99977826002646;0.236599206267813]
Robot Arm 1 togoal: 25.1253183381661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395234713654;-9.96504482896025;-6.01150287117751]
Robot Arm 1 end: [-9.31840827641621;-6.00419213314064;0.249242145458385]
Robot Arm 1 togoal: 25.1187665343937
Mobile Platform 2 Pose: [-2.34448576254291;9.96659211863206;5.81163615679385]
Robot Arm 2 end: [9.32129519256584;5.83649758129315;0.247576491626383]
Robot Arm 1 togoal: 25.0145699741621

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395107194348;-9.96352783523097;-5.85670775549414]
Robot Arm 1 end: [-9.31706418511157;-5.84946658212853;0.249007047084581]
Robot Arm 1 togoal: 25.0194429542705
Mobile Platform 2 Pose: [-2.34448563570609;9.96512531674353;5.64904691956338]
Robot Arm 2 end: [9.31797086839435;5.67391159493059;0.250608799827672]
Robot Arm 1 togoal: 24.909223130385

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785394979408191;-9.96200538177151;-5.70281359519745]
Robot Arm 1 end: [-9.31571415631548;-5.69564208435048;0.248772796220215]
Robot Arm 1 togoal: 24.9212470768731
Mobile Platform 2 Pose: [-2.34444533964584;9.96365130485924;5.48741597084555]
Robot Arm 2 end: [9.38853006080465;5.50944927998193;0.165674610733544]
Robot Arm 1 togoal: 24.8655388398854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785394851359011;-9.96047748743221;-5.54982575676613]
Robot Arm 1 end: [-9.31435817613277;-5.54272400392155;0.248539434314448]
Robot Arm 1 togoal: 24.8241788808663
Mobile Platform 2 Pose: [-2.34218676358823;9.96216739916047;5.32704909912855]
Robot Arm 2 end: [9.35189322741443;5.35035439427254;0.200267526642795]
Robot Arm 1 togoal: 24.7360766044494

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785394723050715;-9.95894417207523;-5.39774949721704]
Robot Arm 1 end: [-9.31299623113679;-5.3907175954656;0.248307004475561]
Robot Arm 1 togoal: 24.7282380890377
Mobile Platform 2 Pose: [-2.34006738488636;9.96068619244855;5.16768986016358]
Robot Arm 2 end: [9.32389185879764;5.19190953076416;0.235703159229182]
Robot Arm 1 togoal: 24.6142011800165

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.78539459448729;-9.9574054565693;-5.24658996030585]
Robot Arm 1 end: [-9.31162830882408;-5.23962800032054;0.24807555010829]
Robot Arm 1 togoal: 24.6334241667995
Mobile Platform 2 Pose: [-2.33995064779759;9.95919426696216;5.00913021099356]
Robot Arm 2 end: [9.37899657021846;5.03113394713424;0.169842142452814]
Robot Arm 1 togoal: 24.5622906782173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785394465672798;-9.9558613627848;-5.09635217280335]
Robot Arm 1 end: [-9.31025439774191;-5.0894602428164;0.247845114515653]
Robot Arm 1 togoal: 24.5397363206592
Mobile Platform 2 Pose: [-2.33823185864222;9.95769373258598;4.85179735895581]
Robot Arm 2 end: [9.34416091012979;4.87499180081876;0.204202982974794]
Robot Arm 1 togoal: 24.4375918775862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785394336611381;-9.95431191358804;-4.94704104083263]
Robot Arm 1 end: [-9.30887448753664;-4.94021922661363;0.247615740762273]
Robot Arm 1 togoal: 24.4471734968768
Mobile Platform 2 Pose: [-2.33823172988558;9.95619339949911;4.69544258523627]
Robot Arm 2 end: [9.31890121536294;4.71945956582255;0.236400307430564]
Robot Arm 1 togoal: 24.3214438397268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785394207307253;-9.95275713283489;-4.79866134626694]
Robot Arm 1 end: [-9.30748856898353;-4.79190973110208;0.247387471607786]
Robot Arm 1 togoal: 24.3557343803668
Mobile Platform 2 Pose: [-2.33823160146191;9.95468244957881;4.539915913272]
Robot Arm 2 end: [9.31083527698708;4.56410983282489;0.24596321542342]
Robot Arm 1 togoal: 24.22080455145

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785394077764703;-9.95119704536328;-4.65121774319257]
Robot Arm 1 end: [-9.30609663401222;-4.64453640786498;0.247160349458774]
Robot Arm 1 togoal: 24.2654173938631
Mobile Platform 2 Pose: [-2.33823147299997;9.95316399766312;4.38535513065546]
Robot Arm 2 end: [9.3075471273965;4.40954687321434;0.248617836537707]
Robot Arm 1 togoal: 24.1254092926502

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785393947988093;-9.94963167698491;-4.50471475444215]
Robot Arm 1 end: [-9.30469867509091;-4.49810377720713;0.246934417319615]
Robot Arm 1 togoal: 24.1762206967858
Mobile Platform 2 Pose: [-2.33823134438096;9.95163894917908;4.23180617188486]
Robot Arm 2 end: [9.30563986616604;4.25594391421164;0.249212990809221]
Robot Arm 1 togoal: 24.032417891756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785393817981852;-9.94806105447567;-4.35915676819512]
Robot Arm 1 end: [-9.3032946912947;-4.35261622481175;0.246709708469777]
Robot Arm 1 togoal: 24.0881421903555
Mobile Platform 2 Pose: [-2.33823121557097;9.95010759856947;4.07928611008749]
Robot Arm 2 end: [9.30413337795657;4.10335464947707;0.249199065592133]
Robot Arm 1 togoal: 23.9410036435322

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.785393687750482;-9.94648520556856;-4.21454803477975]
Robot Arm 1 end: [-9.30188466403895;-4.20807799833421;0.246486291178773]
Robot Arm 1 togoal: 24.0011794956953
Mobile Platform 2 Pose: [-2.33823108656299;9.94857006196705;3.9278032125405]
Robot Arm 2 end: [9.30274000479731;3.95179806308169;0.249006298057652]
Robot Arm 1 togoal: 23.8509205034857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.78539355729855;-9.94490415892622;-4.07089266295171]
Robot Arm 1 end: [-9.30046861631089;-4.0644932041782;0.246264156396858]
Robot Arm 1 togoal: 23.9153300075609
Mobile Platform 2 Pose: [-2.3382309573584;9.94702640306163;3.7773630010096]
Robot Arm 2 end: [9.30137489986089;3.80128283042149;0.248762005260871]
Robot Arm 1 togoal: 23.7620931381376

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785393426630689;-9.94331794416529;-3.92819461787958]
Robot Arm 1 end: [-9.29904652254849;-3.92186580471892;0.246043385115365]
Robot Arm 1 togoal: 23.8305908137824
Mobile Platform 2 Pose: [-2.33823082796123;9.94547667057429;3.62797005415378]
Robot Arm 2 end: [9.30001300711595;3.65181446125675;0.248503851254427]
Robot Arm 1 togoal: 23.6744965439182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785393295751596;-9.94172659180549;-3.78645771690039]
Robot Arm 1 end: [-9.29761837310931;-3.78019961462126;0.245824031572686]
Robot Arm 1 togoal: 23.7469587535631
Mobile Platform 2 Pose: [-2.33823069837633;9.94392090948203;3.4796285395049]
Robot Arm 2 end: [9.29864692930559;3.50339739997474;0.248242995845383]
Robot Arm 1 togoal: 23.5881204218408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.78539316466603;-9.94013013326573;-3.64568562693255]
Robot Arm 1 end: [-9.29618420117679;-3.63949829849306;0.245606084672]
Robot Arm 1 togoal: 23.6644304431692
Mobile Platform 2 Pose: [-2.33823056860889;9.94235916437452;3.33234236873332]
Robot Arm 2 end: [9.29727448710346;3.35603564244541;0.247982766828631]
Robot Arm 1 togoal: 23.5029586135201

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785393033378809;-9.93852860086262;-3.50588186227971]
Robot Arm 1 end: [-9.29474398987209;-3.49976536783994;0.2453896095672]
Robot Arm 1 togoal: 23.583002194157
Mobile Platform 2 Pose: [-2.33823043866418;9.94079148044576;3.18611524151858]
Robot Arm 2 end: [9.29589504785872;3.20973291567466;0.247724168755468]
Robot Arm 1 togoal: 23.4190059725828

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.786644763108853;-9.93692201851588;-3.367049768949]
Robot Arm 1 end: [-9.29329786938941;-3.3609901034702;0.245174638783145]
Robot Arm 1 togoal: 23.5026621805773
Mobile Platform 2 Pose: [-2.33814450251605;9.93921790378105;3.04095065671395]
Robot Arm 2 end: [9.37027983531374;3.06171647782965;0.159884058293888]
Robot Arm 1 togoal: 23.4023249881787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.787898148642708;-9.93531042885272;-3.22919451647298]
Robot Arm 1 end: [-9.29184595863643;-3.22319175575776;0.244960880508288]
Robot Arm 1 togoal: 23.4234155608393
Mobile Platform 2 Pose: [-2.33578895085819;9.93763443558951;2.89708387435528]
Robot Arm 2 end: [9.33475527116773;2.91902699694857;0.191260602006921]
Robot Arm 1 togoal: 23.2916772004596

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.789152881763042;-9.93369385299181;-3.0923191153869]
Robot Arm 1 end: [-9.29038804750352;-3.08637333427636;0.24474865319554]
Robot Arm 1 togoal: 23.3452576676195
Mobile Platform 2 Pose: [-2.33335329947296;9.93605717868981;2.75436130205017]
Robot Arm 2 end: [9.30704706019076;2.77714983614427;0.224597553983571]
Robot Arm 1 togoal: 23.1883221346811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.790408606826899;-9.93207234132174;-2.95642642930014]
Robot Arm 1 end: [-9.2889241352392;-2.9505377038189;0.244538012265005]
Robot Arm 1 togoal: 23.2681837778367
Mobile Platform 2 Pose: [-2.33318042632064;9.93447017319028;2.61254426069519]
Robot Arm 2 end: [9.36174677277835;2.63323107209853;0.160991453438282]
Robot Arm 1 togoal: 23.1587000641433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.79166492421527;-9.93044591618399;-2.82151914675422]
Robot Arm 1 end: [-9.28745419370024;-2.8156875546581;0.24432902914919]
Robot Arm 1 togoal: 23.19218889067
Mobile Platform 2 Pose: [-2.33074507381024;9.93287613465808;2.47199885992533]
Robot Arm 2 end: [9.32849783701863;2.49375323289682;0.192157977649877]
Robot Arm 1 togoal: 23.0532106137197

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.792921361220167;-9.92881461492399;-2.68759980224724]
Robot Arm 1 end: [-9.28597824007606;-2.68182542406392;0.244121726351008]
Robot Arm 1 togoal: 23.1172678076117
Mobile Platform 2 Pose: [-2.32840013785363;9.93128380312583;2.33251210460041]
Robot Arm 2 end: [9.30174148035045;2.35506292930398;0.225130421453008]
Robot Arm 1 togoal: 22.9539556966974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.79417735922457;-9.92717848578792;-2.55467075033307]
Robot Arm 1 end: [-9.28449628525085;-2.54895367014413;0.243916150351548]
Robot Arm 1 togoal: 23.0434150756851
Mobile Platform 2 Pose: [-2.32821129634327;9.92968020521365;2.19392566221056]
Robot Arm 2 end: [9.37937137830799;2.21357017884668;0.140009129619303]
Robot Arm 1 togoal: 22.9486049100173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.795432254337051;-9.92553755268772;-2.42273417890615]
Robot Arm 1 end: [-9.28300832114643;-2.41707448553306;0.243712337297872]
Robot Arm 1 togoal: 22.9706249907566
Mobile Platform 2 Pose: [-2.32624672071924;9.92806650059797;2.05664552759716]
Robot Arm 2 end: [9.34263176376945;2.07749813333367;0.169363135119509]
Robot Arm 1 togoal: 22.8436049458916

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.796685276062582;-9.92389187100784;-2.29179206085806]
Robot Arm 1 end: [-9.28151439097061;-2.28618984922292;0.243510302230488]
Robot Arm 1 togoal: 22.8988916358033
Mobile Platform 2 Pose: [-2.32456949702819;9.92646268870909;1.92051625285723]
Robot Arm 2 end: [9.31397241347709;1.94223572132702;0.200963290078595]
Robot Arm 1 togoal: 22.7461869623863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.79793550232863;-9.92224147316276;-2.16184626305896]
Robot Arm 1 end: [-9.28001450236537;-2.15630163572728;0.243310075525986]
Robot Arm 1 togoal: 22.828208870021
Mobile Platform 2 Pose: [-2.3232400282408;9.92484988357916;1.78533360770657]
Robot Arm 2 end: [9.29024456327494;1.80773117264296;0.2322935687737]
Robot Arm 1 togoal: 22.6538476869217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.799181808770398;-9.92058640826669;-2.03289842891151]
Robot Arm 1 end: [-9.27850866078457;-2.02741149815447;0.243111712370394]
Robot Arm 1 togoal: 22.7585702979239
Mobile Platform 2 Pose: [-2.32231048765193;9.92322325339561;1.65103507005377]
Robot Arm 2 end: [9.28262156613551;1.67355876320888;0.241461453586675]
Robot Arm 1 togoal: 22.5771718236063

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.800422850223742;-9.91892671542631;-1.90495004957644]
Robot Arm 1 end: [-9.27699689980084;-1.89952094014242;0.242915219763632]
Robot Arm 1 togoal: 22.6899693429782
Mobile Platform 2 Pose: [-2.32182108354659;9.92158893908004;1.51775877720649]
Robot Arm 2 end: [9.27939136738926;1.54026235588108;0.243985670560131]
Robot Arm 1 togoal: 22.5056293477237

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.801656963875772;-9.91726242392468;-1.77800241022349]
Robot Arm 1 end: [-9.27547921735506;-1.77263126257683;0.242720630479102]
Robot Arm 1 togoal: 22.6223991665303
Mobile Platform 2 Pose: [-2.32311672372994;9.91994863376745;1.38555145589379]
Robot Arm 2 end: [9.27740955727677;1.40801207789384;0.24455995861414]
Robot Arm 1 togoal: 22.436366862476

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.801656829533162;-9.91559359794691;-1.65205664738009]
Robot Arm 1 end: [-9.27395554618429;-1.64675734354917;0.242527966361476]
Robot Arm 1 togoal: 22.5558597654047
Mobile Platform 2 Pose: [-2.32439901186589;9.91830292708961;1.25442530792493]
Robot Arm 2 end: [9.27578302189546;1.27683050865728;0.244567025134922]
Robot Arm 1 togoal: 22.3685493174569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.801656695066123;-9.91392027913012;-1.52711177451036]
Robot Arm 1 end: [-9.27242584004471;-1.52188433309537;0.242337495913384]
Robot Arm 1 togoal: 22.4903355720765
Mobile Platform 2 Pose: [-2.32566546620976;9.91665199938365;1.12438513860149]
Robot Arm 2 end: [9.27425456453604;1.14673119313114;0.244410604353947]
Robot Arm 1 togoal: 22.3019323332203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.801656560480159;-9.91224251131577;-1.4031685787442]
Robot Arm 1 end: [-9.27089025272403;-1.39801301683247;0.24214907355113]
Robot Arm 1 togoal: 22.4258193062482
Mobile Platform 2 Pose: [-2.32691290812124;9.91499596160005;0.995432630313167]
Robot Arm 2 end: [9.27274964423388;1.01771831323115;0.244208475325878]
Robot Arm 1 togoal: 22.2364383900953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.801656425780784;-9.91056033885265;-1.2802276679852]
Robot Arm 1 end: [-9.2693488567463;-1.27514400041445;0.241962656285583]
Robot Arm 1 togoal: 22.3623034171278
Mobile Platform 2 Pose: [-2.32813736234725;9.91333488518573;0.867568281935842]
Robot Arm 2 end: [9.2712466944445;0.889793057407777;0.243994893903395]
Robot Arm 1 togoal: 22.1720383373361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.801656290973526;-9.90887380658661;-1.15828947033935]
Robot Arm 1 end: [-9.26780168823216;-1.15327770939835;0.241778251654397]
Robot Arm 1 togoal: 22.2997801298902
Mobile Platform 2 Pose: [-2.32933372005361;9.91166883122406;0.740792262019473]
Robot Arm 2 end: [9.26973952045246;0.762955750622007;0.243779797800077]
Robot Arm 1 togoal: 22.1087170392323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.801656156063925;-9.90718295983297;-1.03735423372925]
Robot Arm 1 end: [-9.2662487830587;-1.03241438913599;0.241595865250603]
Robot Arm 1 togoal: 22.2382414854309
Mobile Platform 2 Pose: [-2.33049536151961;9.90999785962278;0.615104370381539]
Robot Arm 2 end: [9.26822631473905;0.637206184486536;0.243566145336459]
Robot Arm 1 togoal: 22.04646316261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.801656021057533;-9.90548784435541;-0.917422026103835]
Robot Arm 1 end: [-9.2646901691744;-0.912554104994368;0.241415522698764]
Robot Arm 1 togoal: 22.1776793385436
Mobile Platform 2 Pose: [-2.33161370551044;9.90832202984223;0.490504183314043]
Robot Arm 2 end: [9.26670658894699;0.512543860969926;0.243354805165812]
Robot Arm 1 togoal: 21.9852663949581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.801655885959908;-9.90378850632204;-0.798492735293525]
Robot Arm 1 end: [-9.26312588652014;-0.793696742237942;0.241237220641091]
Robot Arm 1 togoal: 22.118085384019
Mobile Platform 2 Pose: [-2.33267758777458;9.90664141211477;0.366991130556496]
Robot Arm 2 end: [9.2651802810122;0.388968092985586;0.243145977635921]
Robot Arm 1 togoal: 21.9251166519005

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.801655750776615;-9.90208499230422;-0.680566070043894]
Robot Arm 1 end: [-9.2615559529477;-0.675842006992746;0.241061004292432]
Robot Arm 1 togoal: 22.0594511291541
Mobile Platform 2 Pose: [-2.33367253370863;9.90495604317907;0.244564392099448]
Robot Arm 2 end: [9.2636473404488;0.266477905490292;0.242939784244605]
Robot Arm 1 togoal: 21.8660036439263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.801655615513222;-9.90037734921353;-0.563641559697143]
Robot Arm 1 end: [-9.25998040722423;-0.55898942608543;0.2408868808386]
Robot Arm 1 togoal: 22.0017679422517
Mobile Platform 2 Pose: [-2.33457995871858;9.903265993609;0.123223019841823]
Robot Arm 2 end: [9.26210781139154;0.145072147789799;0.242736247979322]
Robot Arm 1 togoal: 21.8079170128938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.801655480175296;-9.89866562429446;-0.447718555497866]
Robot Arm 1 end: [-9.2583992870818;-0.443138348160933;0.240714846424173]
Robot Arm 1 togoal: 21.945027038807
Mobile Platform 2 Pose: [-2.33537667885718;9.90157132343399;0.00296583072409487]
Robot Arm 2 end: [9.26056175728848;0.0247493823665059;0.242535345117604]
Robot Arm 1 togoal: 21.7508462193857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.801655344768401;-9.89694986512267;-0.332796231960915]
Robot Arm 1 end: [-9.25681264941879;-0.328287945410249;0.240544894080782]
Robot Arm 1 togoal: 21.8892195058754
Mobile Platform 2 Pose: [-2.33603517455189;9.89987209245256;-0.116208502585871]
Robot Arm 2 end: [9.2590091825623;-0.0944920183715312;0.242337137165708]
Robot Arm 1 togoal: 21.6947805193995

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.801655209298097;-9.89523011953377;-0.218873587210733]
Robot Arm 1 end: [-9.2552204957403;-0.214437213171867;0.2403770782476]
Robot Arm 1 togoal: 21.8343362388034
Mobile Platform 2 Pose: [-2.33652553972041;9.89816836082648;-0.23430149083746]
Robot Arm 2 end: [9.25745016025067;-0.212653893457658;0.242141582740213]
Robot Arm 1 togoal: 21.6397090861958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.801947990350056;-9.89350643563624;-0.105949443304788]
Robot Arm 1 end: [-9.25362288600556;-0.10158169973238;0.24021141462656]
Robot Arm 1 togoal: 21.780366536707
Mobile Platform 2 Pose: [-2.33681989021459;9.89646018802745;-0.351314749927983]
Robot Arm 2 end: [9.25588473351542;-0.329738162461678;0.241948687485726]
Robot Arm 1 togoal: 21.585620981038

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.802026766645842;-9.8917788603231;0.00597711512698922]
Robot Arm 1 end: [-9.25201989458277;0.0102738424516103;0.240047809522966]
Robot Arm 1 togoal: 21.7273039569686
Mobile Platform 2 Pose: [-2.33689878648712;9.89474763383826;-0.467250074211323]
Robot Arm 2 end: [9.25431293884364;-0.445746827744257;0.241758472899482]
Robot Arm 1 togoal: 21.532505179638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.801884828197628;-9.89004744266395;0.116907900173092]
Robot Arm 1 end: [-9.25041146791218;0.121131160649521;0.239886395913391]
Robot Arm 1 togoal: 21.6751388299035
Mobile Platform 2 Pose: [-2.33689865394335;9.89303075767449;-0.582109399326107]
Robot Arm 2 end: [9.25273490443177;-0.560680291550529;0.241570906430348]
Robot Arm 1 togoal: 21.4803514092326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.801531805139766;-9.88831221997766;0.226844965935283]
Robot Arm 1 end: [-9.24879762390701;0.230992426141518;0.239727201231914]
Robot Arm 1 togoal: 21.6238613325441
Mobile Platform 2 Pose: [-2.33689852139125;9.89130962062727;-0.695895117380014]
Robot Arm 2 end: [9.25115069247257;-0.674540057219735;0.241386023332952]
Robot Arm 1 togoal: 21.4291487945268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.800990302080709;-9.886573268039;0.335790505339935]
Robot Arm 1 end: [-9.24717845620268;0.339860093306459;0.23957019283728]
Robot Arm 1 togoal: 21.5734615649063
Mobile Platform 2 Pose: [-2.33689838883628;9.88958428392702;-0.808609909041925]
Robot Arm 2 end: [9.24956033821056;-0.787328802330553;0.241203854055049]
Robot Arm 1 togoal: 21.3788858920654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.800289480361181;-9.88483063834328;0.443746917507451]
Robot Arm 1 end: [-9.24555400811115;0.44773689517707;0.239415382499159]
Robot Arm 1 togoal: 21.523929442244
Mobile Platform 2 Pose: [-2.33689825628384;9.88785480877202;-0.920256640072216]
Robot Arm 2 end: [9.24796390851422;-0.899049390656885;0.241024389679046]
Robot Arm 1 togoal: 21.3295511950014

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.799459019230289;-9.88308436583948;0.550716520562178]
Robot Arm 1 end: [-9.24392429901393;0.554625488505796;0.239262800887247]
Robot Arm 1 togoal: 21.4752548840789
Mobile Platform 2 Pose: [-2.33689812373931;9.88612125629079;-1.03083835820954]
Robot Arm 2 end: [9.24636144189545;-1.00970486697667;0.240847650297468]
Robot Arm 1 togoal: 21.281133084716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.79852542044166;-9.88133448703226;0.656701878517548]
Robot Arm 1 end: [-9.24228938853421;0.660528739317811;0.239112410414547]
Robot Arm 1 togoal: 21.4274277535484
Mobile Platform 2 Pose: [-2.33644082707955;9.88438368195457;-1.14035831830222]
Robot Arm 2 end: [9.24475284171281;-1.11930358813109;0.240673608463282]
Robot Arm 1 togoal: 21.2336175996287

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.797510621017084;-9.87958105301631;0.76170576695435]
Robot Arm 1 end: [-9.24064931859882;0.765449673479802;0.238964226830197]
Robot Arm 1 togoal: 21.3804378266213
Mobile Platform 2 Pose: [-2.33566062174495;9.88264215510737;-1.24881913691951]
Robot Arm 2 end: [9.24313829797574;-1.22784665760129;0.240502170170953]
Robot Arm 1 togoal: 21.1869941357847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.796432024070152;-9.8778241388836;0.865730895524934]
Robot Arm 1 end: [-9.23900419199218;0.869391199448582;0.238818207064441]
Robot Arm 1 togoal: 21.33427499397
Mobile Platform 2 Pose: [-2.3345723481261;9.88089672897292;-1.35622389847982]
Robot Arm 2 end: [9.24151784912049;-1.33533698104131;0.240333367500767]
Robot Arm 1 togoal: 21.1412511785308

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.795303165600185;-9.87606378020138;0.968780167066827]
Robot Arm 1 end: [-9.23735402249328;0.972356375347068;0.238674383145917]
Robot Arm 1 togoal: 21.2889290051922
Mobile Platform 2 Pose: [-2.33319427153689;9.87914746227932;-1.46257585226192]
Robot Arm 2 end: [9.23989157373587;-1.44177759232531;0.240167173309056]
Robot Arm 1 togoal: 21.0963772074323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.794134444784138;-9.87430002757127;1.07085671292868]
Robot Arm 1 end: [-9.2356988825878;1.07434845220413;0.238532715955302]
Robot Arm 1 togoal: 21.2443895979363
Mobile Platform 2 Pose: [-2.33314226286734;9.87739441522749;-1.56787852332353]
Robot Arm 2 end: [9.32646935943503;-1.55004066336848;0.142852610842162]
Robot Arm 1 togoal: 21.1379639728653

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.792933870041173;-9.87253292863602;1.17196363077058]
Robot Arm 1 end: [-9.23403881537866;1.17537062034238;0.238393219634894]
Robot Arm 1 togoal: 21.2006465229919
Mobile Platform 2 Pose: [-2.33125233298866;9.87563224407419;-1.67205650324148]
Robot Arm 2 end: [9.29299720001715;-1.65319960709024;0.168299018837683]
Robot Arm 1 togoal: 21.0646295410906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.791707621215728;-9.87076253279736;1.27210426488223]
Robot Arm 1 end: [-9.23237387930114;1.27542629612103;0.238255870187553]
Robot Arm 1 togoal: 21.1576894733383
Mobile Platform 2 Pose: [-2.3291523511851;9.87388440747279;-1.77503476440828]
Robot Arm 2 end: [9.26644896091742;-1.75546940057505;0.195537656216078]
Robot Arm 1 togoal: 20.9982187528676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.790460515267583;-9.8689888893652;1.37128209438763]
Robot Arm 1 end: [-9.23070412401538;1.37451901525775;0.238120668155061]
Robot Arm 1 togoal: 21.1155081090569
Mobile Platform 2 Pose: [-2.32687937270339;9.87213289670445;-1.87702573898223]
Robot Arm 2 end: [9.24385807656534;-1.85691456580014;0.223879180091824]
Robot Arm 1 togoal: 20.936055896016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.789196314174688;-9.86721204543331;1.46950061789639]
Robot Arm 1 end: [-9.22902958963152;1.47265232104012;0.237987624475847]
Robot Arm 1 togoal: 21.0740921074322
Mobile Platform 2 Pose: [-2.32673014972722;9.87036987001067;-1.97810157231962]
Robot Arm 2 end: [9.33625886098572;-1.96108614051269;0.125914980692007]
Robot Arm 1 togoal: 20.987170009089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.787917976203549;-9.86543205048247;1.56676361093277]
Robot Arm 1 end: [-9.22735035407582;1.56983002443811;0.237856697100472]
Robot Arm 1 togoal: 21.0334311083813
Mobile Platform 2 Pose: [-2.32424611487607;9.86859745941641;-2.07807089951312]
Robot Arm 2 end: [9.30225204290708;-2.06003641694764;0.150198417629784]
Robot Arm 1 togoal: 20.9164780162994

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.787917841224754;-9.86364895341745;1.66307495613769]
Robot Arm 1 end: [-9.22566651705465;1.66607040075065;0.237727904614721]
Robot Arm 1 togoal: 20.9935090649714
Mobile Platform 2 Pose: [-2.3241682039096;9.8668405416835;-2.17689519653079]
Robot Arm 2 end: [9.27430201040731;-2.15809112962777;0.176358019694971]
Robot Arm 1 togoal: 20.8519139355315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.787917706283844;-9.86186279511467;1.75843653475704]
Robot Arm 1 end: [-9.22397822119573;1.76136106078538;0.237601013930608]
Robot Arm 1 togoal: 20.9543223029807
Mobile Platform 2 Pose: [-2.32409017823146;9.86508262791339;-2.2747389411374]
Robot Arm 2 end: [9.25068375374969;-2.25534259550748;0.203859044040792]
Robot Arm 1 togoal: 20.7919071659554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.787917571385758;-9.86007362359951;1.85285245377958]
Robot Arm 1 end: [-9.22228540296269;1.85570611411222;0.237476171843252]
Robot Arm 1 togoal: 20.9158603081515
Mobile Platform 2 Pose: [-2.32401208960999;9.86331366995163;-2.37166119558267]
Robot Arm 2 end: [9.23160803182816;-2.35182367552628;0.228805247088597]
Robot Arm 1 togoal: 20.7369022725085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.787917436535394;-9.85828148666337;1.94632694770017]
Robot Arm 1 end: [-9.22058808005811;1.94910979736441;0.237353419269001]
Robot Arm 1 togoal: 20.8781126479955
Mobile Platform 2 Pose: [-2.32393396572341;9.86153262387008;-2.46769004742906]
Robot Arm 2 end: [9.22512392612876;-2.44778421908555;0.236029233473818]
Robot Arm 1 togoal: 20.6952289923868

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.787917301737609;-9.85648643183957;2.03886437457926]
Robot Arm 1 end: [-9.21888630026083;2.04157647065145;0.237232748072597]
Robot Arm 1 togoal: 20.8410689277677
Mobile Platform 2 Pose: [-2.32385583932567;9.85974585074116;-2.56275597076964]
Robot Arm 2 end: [9.22207648225065;-2.54288295863183;0.238025326853096]
Robot Arm 1 togoal: 20.6577550297726

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.787917166997204;-9.85468850640412;2.13046921210269]
Robot Arm 1 end: [-9.21718012512558;2.13311061359219;0.237114135327842]
Robot Arm 1 togoal: 20.8047187794741
Mobile Platform 2 Pose: [-2.32377770841256;9.85795522067977;-2.65683854312114]
Robot Arm 2 end: [9.2199976526139;-2.63702666216363;0.238505033641718]
Robot Arm 1 togoal: 20.6219901919991

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.78791703231893;-9.85288775735022;2.22114605421393]
Robot Arm 1 end: [-9.21546959715622;2.22371682201904;0.236997593557177]
Robot Arm 1 togoal: 20.7690518339941
Mobile Platform 2 Pose: [-2.32369959069525;9.85616128135989;-2.74993543354814]
Robot Arm 2 end: [9.2181918917224;-2.73019261680947;0.238548484105846]
Robot Arm 1 togoal: 20.5872213002721

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.787916897707485;-9.85108423134852;2.31089960824742]
Robot Arm 1 end: [-9.2137547770865;2.3133998052238;0.236883091812275]
Robot Arm 1 togoal: 20.7340577654795
Mobile Platform 2 Pose: [-2.32362149579402;9.8543642794017;-2.84204998393835]
Robot Arm 2 end: [9.21646083890637;-2.82237838960362;0.238467924885099]
Robot Arm 1 togoal: 20.5532359417915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.78791676316751;-9.84927797477203;2.3997346906234]
Robot Arm 1 end: [-9.21203571083084;2.40216438140616;0.236770641217565]
Robot Arm 1 togoal: 20.69972626117
Mobile Platform 2 Pose: [-2.32354342598827;9.85256429401074;-2.93318730587078]
Robot Arm 2 end: [9.21474772568818;-2.91358745068333;0.238353460146596]
Robot Arm 1 togoal: 20.5199647935163

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.787916628703591;-9.84746903363103;2.48765622433752]
Robot Arm 1 end: [-9.21031246712248;2.49001547548494;0.236660194347288]
Robot Arm 1 togoal: 20.6660470670934
Mobile Platform 2 Pose: [-2.32346536533558;9.85076138594123;-3.02335309745582]
Robot Arm 2 end: [9.21303641251233;-3.0038250330728;0.238230933783625]
Robot Arm 1 togoal: 20.4873797454341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.787916494320257;-9.84565745361906;2.57466923424956]
Robot Arm 1 end: [-9.20858507838785;2.57695811410508;0.236551777685464]
Robot Arm 1 togoal: 20.63300993043
Mobile Platform 2 Pose: [-2.32338732642199;9.84895560527664;-3.11255328050923]
Robot Arm 2 end: [9.2113223165261;-3.09309692547459;0.238107793059077]
Robot Arm 1 togoal: 20.4554644935033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.78791636002198;-9.84384328004146;2.66077884485677]
Robot Arm 1 end: [-9.20685364356794;2.66299742344232;0.236445312405727]
Robot Arm 1 togoal: 20.6006047105242
Mobile Platform 2 Pose: [-2.32330929867389;9.84714703667314;-3.20079390534927]
Robot Arm 2 end: [9.20960421427446;-3.18140914007327;0.237986158882494]
Robot Arm 1 togoal: 20.4242062244937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.787916225813169;-9.84202655783885;2.74599027556459]
Robot Arm 1 end: [-9.20511818893984;2.74813862477665;0.236340819032702]
Robot Arm 1 togoal: 20.5688212415745
Mobile Platform 2 Pose: [-2.32323128642594;9.84533571072751;-3.28808116177162]
Robot Arm 2 end: [9.20788178675558;-3.26876785543753;0.237866604536548]
Robot Arm 1 togoal: 20.393593117166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.787916091698176;-9.84020733156616;2.83030883796134]
Robot Arm 1 end: [-9.20337878008418;2.83238703132728;0.236238273656249]
Robot Arm 1 togoal: 20.53764945006
Mobile Platform 2 Pose: [-2.3231532693187;9.84352167956386;-3.37442130648591]
Robot Arm 2 end: [9.20615500631426;-3.35517932492596;0.237749267561895]
Robot Arm 1 togoal: 20.3636137356012

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.787915957681292;-9.83838564536294;2.91373993222427]
Robot Arm 1 end: [-9.20163546837454;2.91574804502373;0.236137660134239]
Robot Arm 1 togoal: 20.507079305799
Mobile Platform 2 Pose: [-2.32307525220374;9.84170503403521;-3.45982067200503]
Robot Arm 2 end: [9.20442393139955;-3.44064987868961;0.237634198780294]
Robot Arm 1 togoal: 20.3342568177592

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.787915823766747;-9.83656154296493;2.99628904335615]
Robot Arm 1 end: [-9.19988831098673;2.99822715247845;0.236038963798039]
Robot Arm 1 togoal: 20.4771008452665
Mobile Platform 2 Pose: [-2.32299722515275;9.83988577779396;-3.54428567479988]
Robot Arm 2 end: [9.20268857880741;-3.52518593234709;0.237521381553797]
Robot Arm 1 togoal: 20.3055111541016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.787962841304985;-9.83473506767637;3.07796173816318]
Robot Arm 1 end: [-9.19813735905742;3.07983044605246;0.2359421727695]
Robot Arm 1 togoal: 20.4477039868036
Mobile Platform 2 Pose: [-2.32291917314591;9.8380639852821;-3.62782281489636]
Robot Arm 2 end: [9.20094906232024;-3.60879398588821;0.237410759347665]
Robot Arm 1 togoal: 20.2773657201164

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.788378533398577;-9.83290626219244;3.15876358507923]
Robot Arm 1 end: [-9.19638268988892;3.16056706619706;0.235847252285096]
Robot Arm 1 togoal: 20.418877756077
Mobile Platform 2 Pose: [-2.3228411064468;9.83623968974364;-3.71043863667197]
Robot Arm 2 end: [9.19920541468054;-3.69148058232051;0.237302339349463]
Robot Arm 1 togoal: 20.2498095171855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.788698851170947;-9.83107515778942;3.23869965187655]
Robot Arm 1 end: [-9.19462438437633;3.24043691573052;0.235754111974525]
Robot Arm 1 togoal: 20.3906143933968
Mobile Platform 2 Pose: [-2.32276301034541;9.83441294890317;-3.79213979195036]
Robot Arm 2 end: [9.19745770815627;-3.77325237269904;0.237196093219254]
Robot Arm 1 togoal: 20.2228316746595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.788582041080707;-9.82924181593563;3.3177759768602]
Robot Arm 1 end: [-9.19286244337863;3.31944224189376;0.235662819123391]
Robot Arm 1 togoal: 20.3629053034775
Mobile Platform 2 Pose: [-2.32268488646448;9.83258379460111;-3.87293296906833]
Robot Arm 2 end: [9.19570600729064;-3.85411604508933;0.237091979912407]
Robot Arm 1 togoal: 20.1964214283028

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.787933808579708;-9.82740626965085;3.39599906585971]
Robot Arm 1 end: [-9.19109689369296;3.39758851870169;0.235573382728377]
Robot Arm 1 togoal: 20.3357409242909
Mobile Platform 2 Pose: [-2.32260671459733;9.83075230724789;-3.95282488191962]
Robot Arm 2 end: [9.19395041695488;-3.93407831300673;0.23698995681229]
Robot Arm 1 togoal: 20.1705681702887

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.786955438625858;-9.82556857263891;3.47337578230849]
Robot Arm 1 end: [-9.18932777819679;3.47488486321181;0.235485816676303]
Robot Arm 1 togoal: 20.3091106364347
Mobile Platform 2 Pose: [-2.32252849597819;9.82891851777312;-4.03182234738879]
Robot Arm 2 end: [9.19219098906773;-4.01314599276737;0.23688999807285]
Robot Arm 1 togoal: 20.1452613376402

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.786070915813643;-9.82372874836808;3.54991262977028]
Robot Arm 1 end: [-9.18755514996143;3.55134249831044;0.235400109498491]
Robot Arm 1 togoal: 20.2830033102932
Mobile Platform 2 Pose: [-2.32245022171221;9.82708247192689;-4.10993218200131]
Robot Arm 2 end: [9.19042775866897;-4.09132589958368;0.236792123295259]
Robot Arm 1 togoal: 20.120490477526

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.785793928848507;-9.82188684568377;3.62561548877685]
Robot Arm 1 end: [-9.18577915194805;3.62697300599739;0.235316136715688]
Robot Arm 1 togoal: 20.2574079716513
Mobile Platform 2 Pose: [-2.32422093988409;9.82524421656935;-4.18716132546335]
Robot Arm 2 end: [9.18866142479014;-4.16860442970949;0.236696252689418]
Robot Arm 1 togoal: 20.0962518318165

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.785793796149494;-9.82004290068314;3.70048938757555]
Robot Arm 1 end: [-9.18399988172003;3.70177772559772;0.235233811673969]
Robot Arm 1 togoal: 20.2323169699037
Mobile Platform 2 Pose: [-2.32570424147496;9.8234037779536;-4.26351987450278]
Robot Arm 2 end: [9.18689115635782;-4.24501546427481;0.236602596599391]
Robot Arm 1 togoal: 20.0725263966004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.785793663591522;-9.81819695174006;3.77454002139254]
Robot Arm 1 end: [-9.18221736688778;3.77575927197392;0.235153137279739]
Robot Arm 1 togoal: 20.2077222344656
Mobile Platform 2 Pose: [-2.32687671408203;9.82156126971799;-4.33901425504067]
Robot Arm 2 end: [9.18511721442771;-4.32056568376038;0.236510950956821]
Robot Arm 1 togoal: 20.0493044468112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.78579353117811;-9.8163490374537;3.84777358250671]
Robot Arm 1 end: [-9.18043163623273;3.84892383881893;0.235074124764796]
Robot Arm 1 togoal: 20.1836146675824
Mobile Platform 2 Pose: [-2.3277221147614;9.81971668340774;-4.41365103279869]
Robot Arm 2 end: [9.183339681038;-4.3952618263977;0.236421189815771]
Robot Arm 1 togoal: 20.0265761934559

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.785793398912719;-9.81449919592027;3.92019630787229]
Robot Arm 1 end: [-9.17864272854015;3.92127766440902;0.23499677494847]
Robot Arm 1 togoal: 20.1599852786911
Mobile Platform 2 Pose: [-2.32823063908794;9.81787007572513;-4.48743654880181]
Robot Arm 2 end: [9.18155863163186;-4.46911033162758;0.236333284497876]
Robot Arm 1 togoal: 20.0043320466433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.785793266798746;-9.81264746472165;3.99181447599655]
Robot Arm 1 end: [-9.17685069185424;3.99282702862239;0.234921067401469]
Robot Arm 1 togoal: 20.1368251863742
Mobile Platform 2 Pose: [-2.32839820724528;9.81602146919577;-4.56037726759318]
Robot Arm 2 end: [9.17977412025546;-4.54211769895346;0.236247195294247]
Robot Arm 1 togoal: 19.9825625054771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.785793134839523;-9.81079388094135;4.062634403477]
Robot Arm 1 end: [-9.17505559687809;4.0635782493235;0.234846955235797]
Robot Arm 1 togoal: 20.1141256334902
Mobile Platform 2 Pose: [-2.32822574341171;9.81417090792177;-4.63247984239894]
Robot Arm 2 end: [9.1779862305699;-4.61429056095449;0.236162859316664]
Robot Arm 1 togoal: 19.9612581892984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.785793003038317;-9.80893848115297;4.13266244188097]
Robot Arm 1 end: [-9.17325748390636;4.1335376792433;0.234774441341266]
Robot Arm 1 togoal: 20.091877933453
Mobile Platform 2 Pose: [-2.32771842634264;9.81231844845715;-4.70375073424031]
Robot Arm 2 end: [9.17619501667097;-4.68563530993942;0.236080297942605]
Robot Arm 1 togoal: 19.940409875855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.785792871398332;-9.8070813013942;4.20190497538476]
Robot Arm 1 end: [-9.17145640884127;4.20271170389066;0.234703488610824]
Robot Arm 1 togoal: 20.0700735210292
Mobile Platform 2 Pose: [-2.32688540862647;9.81046410087499;-4.77419646256517]
Robot Arm 2 end: [9.17440053454069;-4.75615835384109;0.235999447318317]
Robot Arm 1 togoal: 19.9200084711171

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.785792739922706;-9.80522237720785;4.27036841706423]
Robot Arm 1 end: [-9.16965242219596;4.27110673746361;0.23463408279265]
Robot Arm 1 togoal: 20.0487039300373
Mobile Platform 2 Pose: [-2.3257397655796;9.80860794469809;-4.84382374602351]
Robot Arm 2 end: [9.17260285423935;-4.82586625509855;0.235920326656962]
Robot Arm 1 togoal: 19.9000449744053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.785792608614517;-9.80336174360337;4.33805920660606]
Robot Arm 1 end: [-9.16784556064995;4.33872922084815;0.234566219365113]
Robot Arm 1 togoal: 20.0277607870617
Mobile Platform 2 Pose: [-2.32429889476197;9.80674998580859;-4.91263908334832]
Robot Arm 2 end: [9.17080204004621;-4.89476531061415;0.235842867380783]
Robot Arm 1 togoal: 19.8805105806634

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.785792477476779;-9.80149943507352;4.40498380735402]
Robot Arm 1 end: [-9.16603591531354;4.40558561853293;0.234499819773488]
Robot Arm 1 togoal: 20.0072358833592
Mobile Platform 2 Pose: [-2.32409534456295;9.80489027205811;-4.98064923908491]
Robot Arm 2 end: [9.2561388762978;-4.96533327309858;0.141217170323745]
Robot Arm 1 togoal: 19.9512043278421

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.785792346512437;-9.79963548559873;4.47114870296262]
Robot Arm 1 end: [-9.16422350173239;4.47168241522475;0.234434915447931]
Robot Arm 1 togoal: 19.9871210397915
Mobile Platform 2 Pose: [-2.32142962549467;9.80302233332276;-5.04803645449285]
Robot Arm 2 end: [9.22531333200582;-5.03188446224945;0.163693121408743]
Robot Arm 1 togoal: 19.9031938812069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.78579221572438;-9.79776992862055;4.5365603957754]
Robot Arm 1 end: [-9.16240838097567;4.53702611447318;0.234371459201076]
Robot Arm 1 togoal: 19.9674082353027
Mobile Platform 2 Pose: [-2.31938759911539;9.80117211408931;-5.1144618556825]
Robot Arm 2 end: [9.20026559720805;-5.09774034271275;0.187742430933873]
Robot Arm 1 togoal: 19.8610151929923

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.785792085115431;-9.79590279708395;4.60122540315108]
Robot Arm 1 end: [-9.16059060083306;4.60162323466854;0.234309433931951]
Robot Arm 1 togoal: 19.9480895447662
Mobile Platform 2 Pose: [-2.31806763311235;9.79932266718751;-5.18006660875857]
Robot Arm 2 end: [9.17906800791441;-5.16292671216573;0.212871975513378]
Robot Arm 1 togoal: 19.8228435541557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.785791954688353;-9.79403412340758;4.66515025543181]
Robot Arm 1 end: [-9.15877022721593;4.66548030717582;0.234248795989892]
Robot Arm 1 togoal: 19.9291571731824
Mobile Platform 2 Pose: [-2.3175434127532;9.79746432907274;-5.24490145473565]
Robot Arm 2 end: [9.16608402341989;-5.22755569538238;0.228990662036228]
Robot Arm 1 togoal: 19.7935416124264

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.785791824445845;-9.79216393948988;4.7283414931696]
Robot Arm 1 end: [-9.15694727588525;4.72860387354049;0.234189568965908]
Robot Arm 1 togoal: 19.9106033845918
Mobile Platform 2 Pose: [-2.31785464765449;9.79559864929942;-5.30898419837834]
Robot Arm 2 end: [9.16114074091916;-5.29162435875831;0.233643152880493]
Robot Arm 1 togoal: 19.7730979698219

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.78579169439055;-9.79029227670538;4.79080566520554]
Robot Arm 1 end: [-9.15512181466823;4.79100048361438;0.234131702310367]
Robot Arm 1 togoal: 19.8924206096715
Mobile Platform 2 Pose: [-2.31900121138069;9.79372977748116;-5.37231355903926]
Robot Arm 2 end: [9.15843136198223;-5.35498567713609;0.234941153686539]
Robot Arm 1 togoal: 19.7553941410334

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.785791564525046;-9.78841916591876;4.85254932559611]
Robot Arm 1 end: [-9.15329389847256;4.85267669247373;0.234075160016029]
Robot Arm 1 togoal: 19.8746013780714
Mobile Platform 2 Pose: [-2.32094187168241;9.79185892070791;-5.43489449203377]
Robot Arm 2 end: [9.1563549414501;-5.41760525296723;0.235269291749782]
Robot Arm 1 togoal: 19.7387279153088

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.785791434851851;-9.78654463749099;4.9135790313416]
Robot Arm 1 end: [-9.15146356863911;4.91363905804514;0.234019928954824]
Robot Arm 1 togoal: 19.8571383173916
Mobile Platform 2 Pose: [-2.32130945171698;9.78998646538614;-5.49673443217231]
Robot Arm 2 end: [9.24216339971274;-5.48193541526748;0.140390661504521]
Robot Arm 1 togoal: 19.8136276849697

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.785791305373425;-9.78466872126854;4.97390134041715]
Robot Arm 1 end: [-9.14963086145477;4.97389413918031;0.233966003704634]
Robot Arm 1 togoal: 19.8400241623282
Mobile Platform 2 Pose: [-2.32456772452494;9.78810588517589;-5.55806098006313]
Robot Arm 2 end: [9.21163281864696;-5.5423915695306;0.162488128561]
Robot Arm 1 togoal: 19.768729422569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.78579117609217;-9.78279144658197;5.03352280965429]
Robot Arm 1 end: [-9.14779583304675;5.0334484936797;0.233913343402535]
Robot Arm 1 togoal: 19.8232517814451
Mobile Platform 2 Pose: [-2.32831154728759;9.78624356916303;-5.61849334826911]
Robot Arm 2 end: [9.18667963615347;-5.6022107271012;0.186133846165376]
Robot Arm 1 togoal: 19.7294473718314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.785791047010429;-9.7809128422694;5.09244999211875]
Robot Arm 1 end: [-9.14595852711961;5.09230867542637;0.233861932682651]
Robot Arm 1 togoal: 19.8068141422041
Mobile Platform 2 Pose: [-2.32960906346096;9.78438283846748;-5.67815796529213]
Robot Arm 2 end: [9.16559868498418;-5.661446001709;0.210873873775874]
Robot Arm 1 togoal: 19.6941527925567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.785790918130486;-9.77903293665691;5.15068943544308]
Robot Arm 1 end: [-9.14411900154985;5.150481232979;0.233811727468256]
Robot Arm 1 togoal: 19.7907043398191
Mobile Platform 2 Pose: [-2.33090659528753;9.78251361434854;-5.73709479939852]
Robot Arm 2 end: [9.15174708330021;-5.72014504460298;0.22816742454956]
Robot Arm 1 togoal: 19.6666720837584

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.785790789454567;-9.77715175758418;5.20824767936417]
Robot Arm 1 end: [-9.1422772832837;5.20797270688393;0.233762729919671]
Robot Arm 1 togoal: 19.7749155486837
Mobile Platform 2 Pose: [-2.33220419021902;9.78063677338946;-5.79532678938706]
Robot Arm 2 end: [9.14655670021;-5.77834833774024;0.233169950689676]
Robot Arm 1 togoal: 19.648701801079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.785790660984845;-9.77526933239326;5.26513125425342]
Robot Arm 1 end: [-9.14043339139574;5.26478962821883;0.233714956955004]
Robot Arm 1 togoal: 19.7594410461816
Mobile Platform 2 Pose: [-2.33350180695796;9.77875680441665;-5.85286040329139]
Robot Arm 2 end: [9.14376482786158;-5.83591066235702;0.234575629747739]
Robot Arm 1 togoal: 19.6336002673152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.785790532723441;-9.77338568792059;5.32134667943733]
Robot Arm 1 end: [-9.13858741443477;5.32093851726709;0.2336683170242]
Robot Arm 1 togoal: 19.7442742951254
Mobile Platform 2 Pose: [-2.33479946024571;9.77687505173277;-5.90970146668402]
Robot Arm 2 end: [9.14165161383931;-5.89279649732219;0.234941733062733]
Robot Arm 1 togoal: 19.6195283578368

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.785790404672412;-9.77150085054088;5.37690046015703]
Robot Arm 1 end: [-9.13673939134382;5.37642588000078;0.233622796268343]
Robot Arm 1 togoal: 19.729408816283
Mobile Platform 2 Pose: [-2.33609716108431;9.77499192999258;-5.96585672233206]
Robot Arm 2 end: [9.13973046839986;-5.94900098455748;0.235009219114318]
Robot Arm 1 togoal: 19.6059594948738

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.785790276833764;-9.76961484614018;5.43179908667972]
Robot Arm 1 end: [-9.13488933944812;5.43125820740665;0.233578406195236]
Robot Arm 1 togoal: 19.7148382179644
Mobile Platform 2 Pose: [-2.33723641912644;9.77310765297191;-6.02133291458338]
Robot Arm 2 end: [9.13786262589926;-6.00452934577528;0.234991784574459]
Robot Arm 1 togoal: 19.592738164093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.785790149209451;-9.76772770012549;5.48604903271682]
Robot Arm 1 end: [-9.13303731336854;5.48544197393134;0.233535108504977]
Robot Arm 1 togoal: 19.7005562574341
Mobile Platform 2 Pose: [-2.33878664872439;9.77122217566341;-6.07613655488826]
Robot Arm 2 end: [9.13600860730049;-6.05938084888292;0.23495080928978]
Robot Arm 1 togoal: 19.5798168335088

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.786924960236391;-9.76583941074665;5.53965670909309]
Robot Arm 1 end: [-9.13118330998592;5.53899616263322;0.233492887335655]
Robot Arm 1 togoal: 19.6865539046946
Mobile Platform 2 Pose: [-2.34069101802668;9.76933554486882;-6.13027557293362]
Robot Arm 2 end: [9.13415711803044;-6.11356380292786;0.2349039529081]
Robot Arm 1 togoal: 19.5671765457803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.787171788298033;-9.76395005217189;5.59262662506564]
Robot Arm 1 end: [-9.21786719521158;5.59194236623809;0.138369473616854]
Robot Arm 1 togoal: 19.765349954095
Mobile Platform 2 Pose: [-2.34101649892589;9.76744781062851;-6.1837573016878]
Robot Arm 2 end: [9.2377824696626;-6.16993246528062;0.123788260358522]
Robot Arm 1 togoal: 19.6650423929358

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.789828640913428;-9.76205144312238;5.64428207819136]
Robot Arm 1 end: [-9.18770684916944;5.64366553125867;0.159877850675971]
Robot Arm 1 togoal: 19.7230423017999
Mobile Platform 2 Pose: [-2.34341458147713;9.76555221396453;-6.23691816113103]
Robot Arm 2 end: [9.20698649341094;-6.22223342543843;0.144791377616157]
Robot Arm 1 togoal: 19.6232973942268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.791867738499497;-9.76017150584819;5.69534712027122]
Robot Arm 1 end: [-9.16285262317909;5.69467826052151;0.18291527132371]
Robot Arm 1 togoal: 19.6860657999313
Mobile Platform 2 Pose: [-2.34594821017034;9.76367547989117;-6.28927202686605]
Robot Arm 2 end: [9.17982650565538;-6.27391049454711;0.167201735981654]
Robot Arm 1 togoal: 19.5852561262125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.793184629257176;-9.75829446931143;5.74596630271462]
Robot Arm 1 end: [-9.14189756520326;5.7451804317054;0.207082843502699]
Robot Arm 1 togoal: 19.6530432821387
Mobile Platform 2 Pose: [-2.34854089285193;9.7618073522295;-6.34090857963013]
Robot Arm 2 end: [9.15752527203988;-6.32508355949875;0.191086797879998]
Robot Arm 1 togoal: 19.5520959111469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.793697687885681;-9.75641006143054;5.7962014801323]
Robot Arm 1 end: [-9.12694320810726;5.79529623727613;0.225761297208051]
Robot Arm 1 togoal: 19.6264396545995
Mobile Platform 2 Pose: [-2.35111989272476;9.75993271580606;-6.39186819280972]
Robot Arm 2 end: [9.13765514267428;-6.37566665519503;0.21562811276044]
Robot Arm 1 togoal: 19.5215171623055

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.793359730609647;-9.75451819391851;5.84602557181134]
Robot Arm 1 end: [-9.12142526224784;5.84503372132729;0.231197727485531]
Robot Arm 1 togoal: 19.6101055259481
Mobile Platform 2 Pose: [-2.35361819712367;9.7580475270521;-6.4421724372208]
Robot Arm 2 end: [9.12654345111688;-6.42580016123199;0.22931253745389]
Robot Arm 1 togoal: 19.5004621836461

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.792167260905205;-9.75262347912102;5.89531111268001]
Robot Arm 1 end: [-9.11851940508619;5.89423550324392;0.232746652189268]
Robot Arm 1 togoal: 19.5968028778581
Mobile Platform 2 Pose: [-2.35388001671837;9.75615579892414;-6.49184883373945]
Robot Arm 2 end: [9.22505547645496;-6.47818797899347;0.124560168718436]
Robot Arm 1 togoal: 19.5947942624174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.790163382741177;-9.75072728786591;5.94402465208844]
Robot Arm 1 end: [-9.11635433293233;5.9428600892919;0.233168934289815]
Robot Arm 1 togoal: 19.5845320620915
Mobile Platform 2 Pose: [-2.35604790594605;9.75425596015609;-6.54126838303855]
Robot Arm 2 end: [9.19439136361058;-6.52676088124201;0.145452237655545]
Robot Arm 1 togoal: 19.5545437543812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.789773893879698;-9.74882999332369;5.99216135274854]
Robot Arm 1 end: [-9.20331478261149;5.99103486294712;0.13789638492902]
Robot Arm 1 togoal: 19.6660132136687
Mobile Platform 2 Pose: [-2.35798021702542;9.75237555411518;-6.58992660486665]
Robot Arm 2 end: [9.16784251231929;-6.57478368053158;0.167825197931748]
Robot Arm 1 togoal: 19.5184368087503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.786446444144102;-9.74692360409995;6.03901281966926]
Robot Arm 1 end: [-9.17333936774508;6.03788040319964;0.159175962407378]
Robot Arm 1 togoal: 19.6257458181705
Mobile Platform 2 Pose: [-2.35964134632336;9.75050217450831;-6.63790488263552]
Robot Arm 2 end: [9.14583187940716;-6.62232848392506;0.191595933328867]
Robot Arm 1 togoal: 19.4868893976825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.782651313993618;-9.745036044413;6.08534579226899]
Robot Arm 1 end: [-9.14854972179411;6.08408356880466;0.181964893367062]
Robot Arm 1 togoal: 19.5906705592464
Mobile Platform 2 Pose: [-2.36100349595487;9.74862195248826;-6.68523329854959]
Robot Arm 2 end: [9.12614118668262;-6.66930872174334;0.215984003134081]
Robot Arm 1 togoal: 19.4577965822456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.778536452692338;-9.74315190240509;6.13129486739845]
Robot Arm 1 end: [-9.12767420567192;6.12983786581187;0.205890713973276]
Robot Arm 1 togoal: 19.5595395625108
Mobile Platform 2 Pose: [-2.36204622442448;9.74673107873702;-6.73193085768837]
Robot Arm 2 end: [9.11519811320177;-6.71586082966913;0.229438316602231]
Robot Arm 1 togoal: 19.438222670272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.776438739659337;-9.74126069353331;6.1769184682022]
Robot Arm 1 end: [-9.1121392499205;6.17529446923577;0.225345419905927]
Robot Arm 1 togoal: 19.5340973657942
Mobile Platform 2 Pose: [-2.3627557242081;9.74483378075264;-6.77802965480783]
Robot Arm 2 end: [9.11079919094813;-6.76196107689224;0.233300361104936]
Robot Arm 1 togoal: 19.4259031230034

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.776438615021359;-9.73936180509787;6.2221932351146]
Robot Arm 1 end: [-9.10645683453161;6.22048204879516;0.231014364951039]
Robot Arm 1 togoal: 19.5193624806737
Mobile Platform 2 Pose: [-2.36312405594153;9.74293411912222;-6.82355014343396]
Robot Arm 2 end: [9.10821244163358;-6.80752591777742;0.234381697958195]
Robot Arm 1 togoal: 19.4157425852171

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.776438491472777;-9.73746014577554;6.26697987079041]
Robot Arm 1 end: [-9.10349672928617;6.2651980187562;0.232634480872488]
Robot Arm 1 togoal: 19.5077476617837
Mobile Platform 2 Pose: [-2.36314849332916;9.74103319340314;-6.86850168648275]
Robot Arm 2 end: [9.10613568847841;-6.85253641441482;0.234664767461176]
Robot Arm 1 togoal: 19.406329026882

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.776438368411593;-9.73555712599815;6.31124100729866]
Robot Arm 1 end: [-9.10130821832068;6.30939345696579;0.233081197824333]
Robot Arm 1 togoal: 19.4971610022529
Mobile Platform 2 Pose: [-2.36283089387638;9.73913136577469;-6.91289112134976]
Robot Arm 2 end: [9.10420223516125;-6.89699149624414;0.234719953551938]
Robot Arm 1 togoal: 19.397260661604

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.776438245662232;-9.73365317226648;6.35496979014937]
Robot Arm 1 end: [-9.09933922151352;6.35305810888904;0.233189742968516]
Robot Arm 1 togoal: 19.4870059042866
Mobile Platform 2 Pose: [-2.36217731361176;9.73722872770509;-6.95672408903898]
Robot Arm 2 end: [9.10230825435245;-6.94089452767416;0.234710653012062]
Robot Arm 1 togoal: 19.3884204415752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.7764381231744;-9.7317484231517;6.39816839081892]
Robot Arm 1 end: [-9.09743206046285;6.39619313567241;0.233201420394133]
Robot Arm 1 togoal: 19.4771086252812
Mobile Platform 2 Pose: [-2.36119813736905;9.73532542195868;-7.00000632260621]
Robot Arm 2 end: [9.10042446818469;-6.98425050347757;0.234683564581162]
Robot Arm 1 togoal: 19.3797721829394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.776438000934248;-9.72984293170883;6.44084159252596]
Robot Arm 1 end: [-9.09554164984791;6.43880302006148;0.233185757491893]
Robot Arm 1 togoal: 19.4674163759397
Mobile Platform 2 Pose: [-2.36088983133239;9.73342133897614;-7.04274352369382]
Robot Arm 2 end: [9.18739462154468;-7.02931683823058;0.138951267861366]
Robot Arm 1 togoal: 19.4651259823436

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.776437878938468;-9.72793672534277;6.48299490969118]
Robot Arm 1 end: [-9.09365511598918;6.4808931872184;0.23316276176073]
Robot Arm 1 togoal: 19.4579110698014
Mobile Platform 2 Pose: [-2.35742049010326;9.73150928456473;-7.085354982005]
Robot Arm 2 end: [9.15762402633636;-7.07118254938367;0.160143359747363]
Robot Arm 1 togoal: 19.4279299151715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.776437757186776;-9.72602982357231;6.52463404005283]
Robot Arm 1 end: [-9.0917688327839;6.52246930878228;0.233138128385545]
Robot Arm 1 togoal: 19.4485846547552
Mobile Platform 2 Pose: [-2.35439597537889;9.72961681993525;-7.12725425354363]
Robot Arm 2 end: [9.13292866439224;-7.11259034969237;0.182814420957943]
Robot Arm 1 togoal: 19.3957942453865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.776437635679731;-9.72412224334094;6.56576470442653]
Robot Arm 1 end: [-9.08988173843159;6.56353709764326;0.233113558330008]
Robot Arm 1 togoal: 19.4394319246529
Mobile Platform 2 Pose: [-2.35193565330722;9.72772814990178;-7.16852380221171]
Robot Arm 2 end: [9.11216797540698;-7.15353256027913;0.206611092369663]
Robot Arm 1 togoal: 19.3676265919721

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.776437514418121;-9.72221400055224;6.60639260118638]
Robot Arm 1 end: [-9.08799359886988;6.60410225042999;0.233089452009609]
Robot Arm 1 togoal: 19.4304486498809
Mobile Platform 2 Pose: [-2.35014975552506;9.72583215408539;-7.20919237741763]
Robot Arm 2 end: [9.09635091820166;-7.19399494845253;0.226523869359681]
Robot Arm 1 togoal: 19.3447654726856

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.776437393402764;-9.72030511059576;6.64652339096736]
Robot Arm 1 end: [-9.08610434014152;6.64417042736902;0.233065948521175]
Robot Arm 1 togoal: 19.4216308564516
Mobile Platform 2 Pose: [-2.34912936581151;9.72392773630258;-7.24929033021367]
Robot Arm 2 end: [9.09060194875279;-7.23408721171337;0.232307730222625]
Robot Arm 1 togoal: 19.3329274576903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.776437272634467;-9.71839558847547;6.68616269307586]
Robot Arm 1 end: [-9.08421397509577;6.68374724785063;0.233043060714941]
Robot Arm 1 togoal: 19.4129747447637
Mobile Platform 2 Pose: [-2.34893612127231;9.72202049194791;-7.28886445634125]
Robot Arm 2 end: [9.08762152513849;-7.27370307261097;0.233954690013317]
Robot Arm 1 togoal: 19.3242458081339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.77643715211401;-9.71648544884559;6.72531608378085]
Robot Arm 1 end: [-9.08232250329029;6.72283828816817;0.233020817426301]
Robot Arm 1 togoal: 19.4044765809289
Mobile Platform 2 Pose: [-2.34959397424781;9.72011203558345;-7.32793307464397]
Robot Arm 2 end: [9.08542242867778;-7.31281965180359;0.234407167703196]
Robot Arm 1 togoal: 19.3165630643996

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.776437031842143;-9.71457470601834;6.76398909602257]
Robot Arm 1 end: [-9.08042996662662;6.7614490817042;0.232999176043246]
Robot Arm 1 togoal: 19.3961327558937
Mobile Platform 2 Pose: [-2.35108533902991;9.71820273418546;-7.36650660143249]
Robot Arm 2 end: [9.08344474899538;-7.35143620814403;0.234516764019122]
Robot Arm 1 togoal: 19.3092670177806

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.776436911819581;-9.71266337400719;6.80218721817729]
Robot Arm 1 end: [-9.07853637587267;6.79958511696167;0.232978144879596]
Robot Arm 1 togoal: 19.3879397040618
Mobile Platform 2 Pose: [-2.35143184941546;9.71629285641327;-7.40459325546961]
Robot Arm 2 end: [9.1695701889015;-7.39171121486404;0.138933473551122]
Robot Arm 1 togoal: 19.3955071987669

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.776436792047011;-9.71075146650712;6.83991589433345]
Robot Arm 1 end: [-9.0766417771883;6.83725183850153;0.232957673454267]
Robot Arm 1 togoal: 19.3798939745088
Mobile Platform 2 Pose: [-2.35142043994013;9.71437744538051;-7.44264654770089]
Robot Arm 2 end: [9.13987446192625;-7.42900853670928;0.159993340260763]
Robot Arm 1 togoal: 19.3596779583356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.776436672525084;-9.70883899692943;6.87718052328952]
Robot Arm 1 end: [-9.07474616847838;6.87445464518146;0.232937785556942]
Robot Arm 1 togoal: 19.3719921387859
Mobile Platform 2 Pose: [-2.3514089847192;9.71248222237683;-7.48005214819547]
Robot Arm 2 end: [9.11573749327833;-7.46593764854765;0.182621723312457]
Robot Arm 1 togoal: 19.3293597494714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.776436553254426;-9.70692597837934;6.91398645910242]
Robot Arm 1 end: [-9.07284958741246;6.91119889126379;0.23291845342525]
Robot Arm 1 togoal: 19.3642308843914
Mobile Platform 2 Pose: [-2.35139749445993;9.71058914968325;-7.51687860028131]
Robot Arm 2 end: [9.09521359250037;-7.50243857180317;0.206299615689466]
Robot Arm 1 togoal: 19.3026794080857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.776436434235628;-9.70501242366033;6.95033901041881]
Robot Arm 1 end: [-9.07095204972151;6.94748988560185;0.232899671257779]
Robot Arm 1 togoal: 19.3566069479696
Mobile Platform 2 Pose: [-2.35138593658362;9.70868825514674;-7.55314476665045]
Robot Arm 2 end: [9.0792998383958;-7.53849273758209;0.226407330673376]
Robot Arm 1 togoal: 19.2809406022066

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.776436315469258;-9.70309834529414;6.98624344033628]
Robot Arm 1 end: [-9.06905357273724;6.98333289136345;0.23288143836968]
Robot Arm 1 togoal: 19.3491171383365
Mobile Platform 2 Pose: [-2.35137432906637;9.70677884566042;-7.58887909899805]
Robot Arm 2 end: [9.07352630190134;-7.57421321524228;0.23224362680872]
Robot Arm 1 togoal: 19.2703072135311

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.776436196955853;-9.70118375550849;7.02170496637998]
Robot Arm 1 end: [-9.06715420981868;7.01873312676021;0.232863680934226]
Robot Arm 1 togoal: 19.341758374227
Mobile Platform 2 Pose: [-2.35136266346918;9.70486665517303;-7.62413784951174]
Robot Arm 2 end: [9.0705348192839;-7.60951167293068;0.233906731896486]
Robot Arm 1 togoal: 19.2628227894089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.776436078695916;-9.69926866630357;7.05672875930062]
Robot Arm 1 end: [-9.06525393644029;7.05369576211284;0.232846468368641]
Robot Arm 1 togoal: 19.3345275567881
Mobile Platform 2 Pose: [-2.35135096913021;9.70295333111421;-7.65894032938287]
Robot Arm 2 end: [9.06832695995971;-7.64436863849131;0.234366391147542]
Robot Arm 1 togoal: 19.2563111266587

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.776435960689932;-9.69735308935768;7.09131994483464]
Robot Arm 1 end: [-9.06335280833958;7.08822592369837;0.232829731198542]
Robot Arm 1 togoal: 19.3274217423865
Mobile Platform 2 Pose: [-2.35133917103149;9.70103928157069;-7.69329574489479]
Robot Arm 2 end: [9.06634076092817;-7.67878256651859;0.234480978127576]
Robot Arm 1 togoal: 19.2501566637513

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.776540551554915;-9.69543703838245;7.12548364231618]
Robot Arm 1 end: [-9.06145081194422;7.1223298893673;0.232813502964318]
Robot Arm 1 togoal: 19.3204377997224
Mobile Platform 2 Pose: [-2.35132732143817;9.69912475277616;-7.72721070997065]
Robot Arm 2 end: [9.06441691474118;-7.71275706608154;0.234497149685927]
Robot Arm 1 togoal: 19.2441825527014

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.7767191215509;-9.69352052082788;7.15922459906432]
Robot Arm 1 end: [-9.16111959898941;7.15650339986061;0.124337895267979]
Robot Arm 1 togoal: 19.4212420364037
Mobile Platform 2 Pose: [-2.35131547324135;9.69720979958957;-7.7606907671068]
Robot Arm 2 end: [9.06251017734639;-7.74629684016458;0.234485556897487]
Robot Arm 1 togoal: 19.2383365082951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.780531039007485;-9.69160109122998;7.19159756436136]
Robot Arm 1 end: [-9.13093270612103;7.18888162942582;0.144624024112037]
Robot Arm 1 togoal: 19.3852878112891
Mobile Platform 2 Pose: [-2.35130352131115;9.69529438983692;-7.7937409177516]
Robot Arm 2 end: [9.06060767153689;-7.77940665791674;0.23446640773979]
Robot Arm 1 togoal: 19.2326017034402

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.784048563049437;-9.68970165314718;7.22366311695088]
Robot Arm 1 end: [-9.1056138686712;7.22083295165389;0.166519194871921]
Robot Arm 1 togoal: 19.3541434590331
Mobile Platform 2 Pose: [-2.35129154842823;9.69337854480672;-7.82636642629898]
Robot Arm 2 end: [9.05870578087388;-7.81209171797717;0.23444547200206]
Robot Arm 1 togoal: 19.2269714527788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.787148497355212;-9.68780685980641;7.25550943777006]
Robot Arm 1 end: [-9.08424709580135;7.25250030055106;0.189674325826895]
Robot Arm 1 togoal: 19.3269156841641
Mobile Platform 2 Pose: [-2.35127949690263;9.69146227830754;-7.85857240413655]
Robot Arm 2 end: [9.05680351207012;-7.84435711488028;0.234424363919501]
Robot Arm 1 togoal: 19.2214420170072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.789717130684509;-9.68590538368899;7.28717516937901]
Robot Arm 1 end: [-9.06496302434297;7.2839712917483;0.213393689509185]
Robot Arm 1 togoal: 19.3017994323547
Mobile Platform 2 Pose: [-2.35126741049432;9.68954566561835;-7.89036384562997]
Robot Arm 2 end: [9.05490064459414;-7.87620783768999;0.23440357060619]
Robot Arm 1 togoal: 19.2160105666127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.791638508454474;-9.68399420893725;7.31868627818287]
Robot Arm 1 end: [-9.05359884631775;7.31535014015183;0.227370115007369]
Robot Arm 1 togoal: 19.2852392296733
Mobile Platform 2 Pose: [-2.35125523433973;9.68762872386723;-7.92174580077015]
Robot Arm 2 end: [9.05299713844141;-7.90764893605281;0.234383181640854]
Robot Arm 1 togoal: 19.2106745266673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.792810542471261;-9.68207714100726;7.3499534863842]
Robot Arm 1 end: [-9.04905382447894;7.34654331678511;0.231409488623226]
Robot Arm 1 togoal: 19.276159567316
Mobile Platform 2 Pose: [-2.35124300981306;9.68571138665342;-7.95272301719291]
Robot Arm 2 end: [9.05109292565257;-7.93868515731648;0.234363206733539]
Robot Arm 1 togoal: 19.2054313875087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.793156772252865;-9.68015808022809;7.38087545748769]
Robot Arm 1 end: [-9.04639981852649;7.37740114296125;0.2325610910148]
Robot Arm 1 togoal: 19.2692357304148
Mobile Platform 2 Pose: [-2.35123072782604;9.68379369613012;-7.98330051635161]
Robot Arm 2 end: [9.04918801433582;-7.96932152263855;0.234343705137142]
Robot Arm 1 togoal: 19.2002787350873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.792638306275738;-9.67823811705551;7.41142589704234]
Robot Arm 1 end: [-9.0442797977385;7.40788349708546;0.232881433130241]
Robot Arm 1 togoal: 19.262993288208
Mobile Platform 2 Pose: [-2.35154861067318;9.68187563435885;-8.01348294594666]
Robot Arm 2 end: [9.14858407194659;-8.00180264114071;0.125614793052032]
Robot Arm 1 togoal: 19.3030610142829

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.791261582019009;-9.67631763409564;7.44160159591181]
Robot Arm 1 end: [-9.04231086233406;7.43798337447712;0.232963596097541]
Robot Arm 1 togoal: 19.2570135261156
Mobile Platform 2 Pose: [-2.35388404851355;9.67995583643323;-8.04388146895101]
Robot Arm 2 end: [9.11847772119155;-8.03144165250096;0.145960801184168]
Robot Arm 1 togoal: 19.2686830152319

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.791249504913375;-9.67439683627533;7.47140604931809]
Robot Arm 1 end: [-9.04038426231287;7.46772775579335;0.23297789968803]
Robot Arm 1 togoal: 19.2511723410862
Mobile Platform 2 Pose: [-2.35601741231244;9.67805721379101;-8.07373623662579]
Robot Arm 2 end: [9.09319600405475;-8.0607905413664;0.167915226489217]
Robot Arm 1 togoal: 19.2390619672065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.791237569369379;-9.67247557966982;7.50084049580742]
Robot Arm 1 end: [-9.03846916351589;7.49710240658548;0.232973002076484]
Robot Arm 1 togoal: 19.245436290213
Mobile Platform 2 Pose: [-2.35790669278825;9.67616353529867;-8.10308678366033]
Robot Arm 2 end: [9.07186991764413;-8.08980830353325;0.191123926682905]
Robot Arm 1 togoal: 19.2133709911398

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.791225683701261;-9.67055402208305;7.52990914874963]
Robot Arm 1 end: [-9.03655693996263;7.52611144083134;0.232962890192827]
Robot Arm 1 togoal: 19.2397937704089
Mobile Platform 2 Pose: [-2.3595178634596;9.67426247519304;-8.13193822228107]
Robot Arm 2 end: [9.05264155175388;-8.11839781069913;0.214879302019636]
Robot Arm 1 togoal: 19.189828768806

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.791213888169425;-9.66863211648559;7.55861622601235]
Robot Arm 1 end: [-9.03464506486328;7.55475904801752;0.232951552230207]
Robot Arm 1 togoal: 19.2342399086207
Mobile Platform 2 Pose: [-2.36082465985004;9.67235112124606;-8.16029953081756]
Robot Arm 2 end: [9.04130147150149;-8.14664311521769;0.22889065552131]
Robot Arm 1 togoal: 19.1748700133551

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.791202207941237;-9.66670983823013;7.58696634345228]
Robot Arm 1 end: [-9.03273283524066;7.58304983600006;0.232940094640037]
Robot Arm 1 togoal: 19.2287718170637
Mobile Platform 2 Pose: [-2.36180779814784;9.67043334249563;-8.18822834304155]
Robot Arm 2 end: [9.03677594856221;-8.1745755384616;0.232920000451503]
Robot Arm 1 togoal: 19.1674245287617

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.79119065663876;-9.66478718062628;7.61496363800007]
Robot Arm 1 end: [-9.03082003318083;7.6109879391222;0.232928866985937]
Robot Arm 1 togoal: 19.2233872407035
Mobile Platform 2 Pose: [-2.36245431282144;9.66851348723879;-8.215777357733]
Robot Arm 2 end: [9.03413056650986;-8.20216326691299;0.234060642903014]
Robot Arm 1 togoal: 19.1621125205607

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.791179205338696;-9.66286428642313;7.64261215917292]
Robot Arm 1 end: [-9.02890678074629;7.63857740622083;0.232917906524758]
Robot Arm 1 togoal: 19.2180843326351
Mobile Platform 2 Pose: [-2.36275690491628;9.66659275983714;-8.24296403475228]
Robot Arm 2 end: [9.03201311277386;-8.22940102378176;0.23437357496613]
Robot Arm 1 togoal: 19.1574515898377

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.791167889035609;-9.66094104864834;7.66991671860617]
Robot Arm 1 end: [-9.02699294615587;7.66582304886953;0.23290725803798]
Robot Arm 1 togoal: 19.2128609406343
Mobile Platform 2 Pose: [-2.36271321443494;9.66467172516303;-8.26979581474972]
Robot Arm 2 end: [9.03004422735769;-8.25628998358349;0.234450520860471]
Robot Arm 1 togoal: 19.1530237163435

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.791156713302613;-9.65901757249253;7.69688143012773]
Robot Arm 1 end: [-9.02507864203912;7.69272898093333;0.232896912500017]
Robot Arm 1 togoal: 19.2077152846252
Mobile Platform 2 Pose: [-2.36232546881946;9.66275036796149;-8.29627729891194]
Robot Arm 2 end: [9.02811675338769;-8.28283297863751;0.234460477555123]
Robot Arm 1 togoal: 19.148709452666

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.791145748430253;-9.65709383622006;7.72351003052004]
Robot Arm 1 end: [-9.02316383932131;7.71929893985366;0.232886879571779]
Robot Arm 1 togoal: 19.2026455259278
Mobile Platform 2 Pose: [-2.36198297463777;9.66082875134538;-8.32241276111086]
Robot Arm 2 end: [9.11569997458441;-8.31100023241267;0.138261558797816]
Robot Arm 1 togoal: 19.2399238909621

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.791134935568977;-9.65516976224689;7.74980695392836]
Robot Arm 1 end: [-9.02124846368202;7.74553735906922;0.232877155005615]
Robot Arm 1 togoal: 19.1976497358472
Mobile Platform 2 Pose: [-2.3579720076169;9.65889914434739;-8.3488293729582]
Robot Arm 2 end: [9.08636507533862;-8.33674907218684;0.158952826052436]
Robot Arm 1 togoal: 19.2070535986662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.791124309849785;-9.65324540719694;7.77577622539309]
Robot Arm 1 end: [-9.01933259493458;7.77144826402214;0.232867711765177]
Robot Arm 1 togoal: 19.1927262260645
Mobile Platform 2 Pose: [-2.35418841668419;9.65699006740296;-8.3747147545459]
Robot Arm 2 end: [9.0618208840011;-8.36222172250836;0.181092069561176]
Robot Arm 1 togoal: 19.1789125437814

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',73,1)">73  </a>    dx = fmincon(fun1, X, [], [], [], [], [], [], fun2, options);
X

X =

   -6.8183
    6.9309
   -6.8183
    6.9309
   -0.0005
    0.9007
   -0.0005
   -0.0000
   -0.0005
   -0.9007
   -0.0005

mobile_robot.car.J*X(1:4)

ans =

   -0.0000
    0.0056
    0.6875

s_d

s_d =

         0
         0
   -0.0019
    0.7262
    0.6875
         0

pinv(mobile_robot.J)*s_d

ans =

   -6.8183
    6.9309
   -6.8183
    6.9309
   -0.0005
    0.9007
   -0.0005
   -0.0000
   -0.0005
   -0.9007
   -0.0005

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',34,1)">34  </a>    dx = fmincon(fun1, X, [], [], [], [], [], [], fun2, options);
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785398092430785;-9.99915555300857;-9.72188133634483]
Robot Arm 1 end: [-9.79588722128923;-9.71913091567573;1.4928546578184]
Robot Arm 1 togoal: 27.9414041720857
Mobile Platform 2 Pose: [-2.35619441922565;9.99920063060603;9.71606634504082]
Robot Arm 2 end: [9.80165343180179;9.7241596752303;1.4943453139743]
Robot Arm 1 togoal: 27.9490369064742

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.587511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397974568962;-9.9977435600634;-9.4383649012938]
Robot Arm 1 end: [-9.6642746048835;-9.43390832774837;1.43977037489491]
Robot Arm 1 togoal: 27.6471894933208
Mobile Platform 2 Pose: [-2.35619434852297;9.99839599295279;9.4320763852433]
Robot Arm 2 end: [9.61581955647337;9.44768124869834;1.41565957689635]
Robot Arm 1 togoal: 27.6225230460187

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.587267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397858124664;-9.99632452722748;-9.1544820692066]
Robot Arm 1 end: [-9.54418936370282;-9.14851712678415;1.3608508000225]
Robot Arm 1 togoal: 27.3617743175634
Mobile Platform 2 Pose: [-2.35619427797391;9.99758702464252;9.14800372594665]
Robot Arm 2 end: [9.45556425312779;9.17001138923824;1.28956243513554]
Robot Arm 1 togoal: 27.3141333455326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.869110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.7853977857298;-9.99542340831435;-8.87675532383065]
Robot Arm 1 end: [-9.39913316968498;-8.86900483264145;1.21419523978828]
Robot Arm 1 togoal: 27.0639952754634
Mobile Platform 2 Pose: [-2.35619420743761;9.9967740079604;8.86387302151153]
Robot Arm 2 end: [9.33238004210365;8.89071943619184;1.12398424898079]
Robot Arm 1 togoal: 27.03253263044

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397717969615;-9.99456774582813;-8.59739944628212]
Robot Arm 1 end: [-9.29381686231628;-8.58843594354791;1.02802543193195]
Robot Arm 1 togoal: 26.7959762626545
Mobile Platform 2 Pose: [-2.35619413676432;9.99595669997063;8.57975632859381]
Robot Arm 2 end: [9.25540338197746;8.60952505419965;0.929641703109064]
Robot Arm 1 togoal: 26.7849712857949

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397651045221;-9.99370994788153;-8.31792779876302]
Robot Arm 1 end: [-9.24089839120325;-8.3084656817844;0.818370552741483]
Robot Arm 1 togoal: 26.5688873433164
Mobile Platform 2 Pose: [-2.35619406578561;9.99513453055733;8.29576384528743]
Robot Arm 2 end: [9.2307905702174;8.32631342786726;0.719369543560829]
Robot Arm 1 togoal: 26.5766755106932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397584832427;-9.99284906841064;-8.03851658027998]
Robot Arm 1 end: [-9.23741250590603;-8.02919126522605;0.60435471620039]
Robot Arm 1 togoal: 26.3812436434829
Mobile Platform 2 Pose: [-2.35619399422953;9.99430684780291;8.01202943476081]
Robot Arm 2 end: [9.2503380950857;8.04159531431976;0.512539167522015]
Robot Arm 1 togoal: 26.402493660854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.242128e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397516917753;-9.99197613237607;-7.76051699411654]
Robot Arm 1 end: [-9.27945133560435;-7.75186033338903;0.401418474650631]
Robot Arm 1 togoal: 26.2312647786947
Mobile Platform 2 Pose: [-2.35619392147489;9.993466738617;7.7299605828184]
Robot Arm 2 end: [9.31377777779175;7.75684048612509;0.32062963545043]
Robot Arm 1 togoal: 26.2634400852535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.194852e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.78539744477289;-9.99108860602175;-7.48538550789485]
Robot Arm 1 end: [-9.3537378569824;-7.47775277618966;0.244958984367162]
Robot Arm 1 togoal: 26.1087589099133
Mobile Platform 2 Pose: [-2.35619384750163;9.99261990086829;7.45029741776238]
Robot Arm 2 end: [9.36610421597746;7.47496002048188;0.228036240761947]
Robot Arm 1 togoal: 26.1168906170747

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.739919e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785397370177538;-9.99020184754455;-7.21250389668748]
Robot Arm 1 end: [-9.38199425534974;-7.2053363181368;0.202237617131473]
Robot Arm 1 togoal: 25.950422604595
Mobile Platform 2 Pose: [-2.35619377313303;9.99177337161375;7.17214452633004]
Robot Arm 2 end: [9.38357811600678;7.19596715720534;0.202045630192077]
Robot Arm 1 togoal: 25.945404750758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.656023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785397295032419;-9.98931416424799;-6.94077422576386]
Robot Arm 1 end: [-9.39043300922104;-6.93383704184724;0.18983315476876]
Robot Arm 1 togoal: 25.7782026994663
Mobile Platform 2 Pose: [-2.35619369845098;9.99092329733929;6.89503985248473]
Robot Arm 2 end: [9.3886941708249;6.91850699327233;0.194123082053434]
Robot Arm 1 togoal: 25.7666051874624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.579037e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785397219506788;-9.98842248640474;-6.6699958380394]
Robot Arm 1 end: [-9.39547472078759;-6.66324780917209;0.182199532288796]
Robot Arm 1 togoal: 25.6054809344978
Mobile Platform 2 Pose: [-2.35619362330814;9.99006766238102;6.61889276695285]
Robot Arm 2 end: [9.39112290624003;6.64210955173314;0.189888724426824]
Robot Arm 1 togoal: 25.5880307004027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.580414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785397143672936;-9.9875262739385;-6.40015369503691]
Robot Arm 1 end: [-9.40090111555999;-6.39359630495176;0.174252343972028]
Robot Arm 1 togoal: 25.4353827011602
Mobile Platform 2 Pose: [-2.35619354788204;9.98920600629426;6.34369917100849]
Robot Arm 2 end: [9.39398792267964;6.36665005746313;0.185120895602804]
Robot Arm 1 togoal: 25.4121737486203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.688113e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785397067531222;-9.98662568139973;-6.13126488379832]
Robot Arm 1 end: [-9.40672588989957;-6.12489944835147;0.166023084811129]
Robot Arm 1 togoal: 25.2679511323304
Mobile Platform 2 Pose: [-2.35619347216962;9.98833846010364;6.0694785067523]
Robot Arm 2 end: [9.39729838057741;6.09214795081048;0.179846358233509]
Robot Arm 1 togoal: 25.23907889022

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396991016623;-9.98572085610249;-5.86334575164688]
Robot Arm 1 end: [-9.41295412851617;-5.85717328837552;0.157542341569054]
Robot Arm 1 togoal: 25.1032218944059
Mobile Platform 2 Pose: [-2.35619339616693;9.98746515422178;5.79624968530046]
Robot Arm 2 end: [9.40105885863717;5.81862234139926;0.174092366200805]
Robot Arm 1 togoal: 25.0687865240842

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396914187279;-9.98481194928783;-5.59641215306997]
Robot Arm 1 end: [-9.41958728366949;-5.5904333261042;0.148841033904322]
Robot Arm 1 togoal: 24.9412269504127
Mobile Platform 2 Pose: [-2.35619331986904;9.98658622523934;5.52403119208176]
Robot Arm 2 end: [9.40527234300893;5.54609198686433;0.167887534684678]
Robot Arm 1 togoal: 24.9013349865863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785396836978275;-9.98389911917091;-5.33047951119431]
Robot Arm 1 end: [-9.42662464530338;-5.32469460032029;0.139950684592314]
Robot Arm 1 togoal: 24.7819956200905
Mobile Platform 2 Pose: [-2.35619324320053;9.98570181942728;5.25284108576884]
Robot Arm 2 end: [9.40994098153458;5.27457525880048;0.161262062435679]
Robot Arm 1 togoal: 24.7367610147785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785396759447068;-9.98298253208538;-5.0655628271511]
Robot Arm 1 end: [-9.43406370846513;-5.05997170502074;0.130903397673878]
Robot Arm 1 togoal: 24.6255547865323
Mobile Platform 2 Pose: [-2.35619316629324;9.98481209312421;4.98269696687968]
Robot Arm 2 end: [9.41506617002065;5.00409010619678;0.154247713207122]
Robot Arm 1 togoal: 24.5750997172871

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785396681540302;-9.98206236184646;-4.80167667933751]
Robot Arm 1 end: [-9.44190025594366;-4.79627879261305;0.121731695009876]
Robot Arm 1 togoal: 24.4719288871994
Mobile Platform 2 Pose: [-2.35619308900085;9.98391721275188;4.71361594050396]
Robot Arm 2 end: [9.42064842704501;4.73465402157599;0.146877718279027]
Robot Arm 1 togoal: 24.4163843864638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785396603303989;-9.98113878957772;-4.53883522268217]
Robot Arm 1 end: [-9.45012831454174;-4.53362957538472;0.112468326943726]
Robot Arm 1 togoal: 24.3211398080073
Mobile Platform 2 Pose: [-2.35619301132476;9.98301735526461;4.44561458287458]
Robot Arm 2 end: [9.42668720945895;4.46628401322952;0.139186633053984]
Robot Arm 1 togoal: 24.2606462664672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785396524732832;-9.9802120026612;-4.27705219175814]
Robot Arm 1 end: [-9.45874013054245;-4.27203733108528;0.10314605190819]
Robot Arm 1 togoal: 24.1732067973199
Mobile Platform 2 Pose: [-2.35619293326051;9.98211270755576;4.17870891158891]
Robot Arm 2 end: [9.43318072306503;4.19899658104322;0.131210154754216]
Robot Arm 1 togoal: 24.1079143174546

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785396445777753;-9.97928219408813;-4.01634090964135]
Robot Arm 1 end: [-9.46772614545361;-4.01151491423801;0.0937974094821573]
Robot Arm 1 togoal: 24.0281463850351
Mobile Platform 2 Pose: [-2.35619285479716;9.98120346610387;3.91291436433595]
Robot Arm 2 end: [9.44012574059898;3.93280770025345;0.122984926750637]
Robot Arm 1 togoal: 23.9582149870889

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785396366447026;-9.97834956167087;-3.75671430091206]
Robot Arm 1 end: [-9.47707500107294;-3.75207477200312;0.0844544981093125]
Robot Arm 1 togoal: 23.8859723291975
Mobile Platform 2 Pose: [-2.35619277592766;9.98028983697972;3.64824578290136]
Robot Arm 2 end: [9.44751744028789;3.66773280952656;0.114548308666487]
Robot Arm 1 togoal: 23.8115719998274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785396286773405;-9.97741430672848;-3.49818490956368]
Robot Arm 1 end: [-9.48677356300669;-3.49372896500071;0.0751487385466001]
Robot Arm 1 togoal: 23.7466955837447
Mobile Platform 2 Pose: [-2.35619269674899;9.97937203423483;3.38471740582672]
Robot Arm 2 end: [9.4553492795574;3.40378680592519;0.105938144067756]
Robot Arm 1 togoal: 23.6680061767861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785396206719501;-9.97647663353954;-3.24076492108621]
Robot Arm 1 end: [-9.49680699317654;-3.23648919284119;0.0659106483528842]
Robot Arm 1 togoal: 23.6103243116789
Mobile Platform 2 Pose: [-2.35619261709592;9.97845027982935;3.12234286681488]
Robot Arm 2 end: [9.46361289465155;3.1409840447701;0.0971924789100496]
Robot Arm 1 togoal: 23.5275352817685

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785396126295735;-9.97553674809382;-2.98446618871351]
Robot Arm 1 end: [-9.50715881424261;-2.98036682362992;0.0567696437545629]
Robot Arm 1 togoal: 23.4768638965307
Mobile Platform 2 Pose: [-2.3561925370103;9.97752480265587;2.86113520171086]
Robot Arm 2 end: [9.47229800902799;2.87933834729793;0.0883493248242023]
Robot Arm 1 togoal: 23.3901738765161

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785396045524069;-9.97459485700816;-2.72930026213804]
Robot Arm 1 end: [-9.51781102594029;-2.72537292625618;0.0477538330889624]
Robot Arm 1 togoal: 23.3463170034361
Mobile Platform 2 Pose: [-2.35619245639681;9.97659583735136;2.60110686163068]
Robot Arm 2 end: [9.48139240818259;2.61886301210526;0.0794463615043106]
Robot Arm 1 togoal: 23.2559332380477

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785395964387958;-9.97365116670994;-2.47527841918102]
Robot Arm 1 end: [-9.52874422345316;-2.47151830544499;0.0388898382922949]
Robot Arm 1 togoal: 23.2186836474323
Mobile Platform 2 Pose: [-2.35619237536391;9.97566362317567;2.34226973237327]
Robot Arm 2 end: [9.49088193761614;2.35957083193919;0.0705206571035813]
Robot Arm 1 togoal: 23.1248213004984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785395882885811;-9.97270588224284;-2.22241169920084]
Robot Arm 1 end: [-9.53993773970099;-2.21881353853792;0.0302026328063705]
Robot Arm 1 togoal: 23.0939612875526
Mobile Platform 2 Pose: [-2.35619229385457;9.97472840327919;2.08463516035832]
Robot Arm 2 end: [9.50075050776826;2.10147411668131;0.0616084179599182]
Robot Arm 1 togoal: 22.9968426083378

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785395801035194;-9.97175920648029;-1.97071093730477]
Robot Arm 1 end: [-9.55136981131163;-1.96726901313147;0.0217154088831381]
Robot Arm 1 togoal: 22.9721449459228
Mobile Platform 2 Pose: [-2.35619221185862;9.97379042314693;1.82821398279633]
Robot Arm 2 end: [9.51098018610616;1.84458471753968;0.0527447004394301]
Robot Arm 1 togoal: 22.8719983512217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785395622931512;-9.9703169873817;-1.72068115083222]
Robot Arm 1 end: [-9.56252340904274;-1.71738931733055;0.0134494491001171]
Robot Arm 1 togoal: 22.8530576336462
Mobile Platform 2 Pose: [-2.35619212930852;9.97284992955062;1.57301656306798]
Robot Arm 2 end: [9.52155125556728;1.58891405775056;0.0439631817010906]
Robot Arm 1 togoal: 22.7502863767531

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.78539544406986;-9.96749220734044;-1.47321617860782]
Robot Arm 1 end: [-9.57248600695588;-1.47006786755378;0.00542502880067247]
Robot Arm 1 togoal: 22.7363504739019
Mobile Platform 2 Pose: [-2.35619204624301;9.97190716933764;1.31905282934221]
Robot Arm 2 end: [9.53244235983459;1.33447316350231;0.0352959179476764]
Robot Arm 1 togoal: 22.6317012850082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785395167739512;-9.96432690272019;-1.2293549851989]
Robot Arm 1 end: [-9.58227040079072;-1.22634321410268;-0.00233812668501604]
Robot Arm 1 togoal: 22.6233652461062
Mobile Platform 2 Pose: [-2.35619196261058;9.97096238819677;1.0663323155956]
Robot Arm 2 end: [9.54363064020499;1.08127269788204;0.026773134504866]
Robot Arm 1 togoal: 22.5162345238087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785394890276807;-9.96115989684119;-0.989425566318198]
Robot Arm 1 end: [-9.59218548860636;-0.986542964629588;-0.0098225145812682]
Robot Arm 1 togoal: 22.5144681506084
Mobile Platform 2 Pose: [-2.35619187842882;9.97001582947131;0.814864204578697]
Robot Arm 2 end: [9.55509191343779;0.829322995406747;0.0184230453452706]
Robot Arm 1 togoal: 22.4038745262177

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785394611713202;-9.95799194457805;-0.75341387106192]
Robot Arm 1 end: [-9.60220203827519;-0.750652716076914;-0.0170132047092903]
Robot Arm 1 togoal: 22.4095591922044
Mobile Platform 2 Pose: [-2.35619169477753;9.9682029254016;0.565522178908123]
Robot Arm 2 end: [9.56593605881071;0.579498903854889;0.0102716870904409]
Robot Arm 1 togoal: 22.2942582507147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.785394325102637;-9.95482376407932;-0.521304054207709]
Robot Arm 1 end: [-9.61229117724994;-0.518656310175313;-0.0238979755818234]
Robot Arm 1 togoal: 22.3085366070431
Mobile Platform 2 Pose: [-2.35619141027684;9.96503835628793;0.319043289402021]
Robot Arm 2 end: [9.57564805270198;0.332538750871215;0.00234445081178586]
Robot Arm 1 togoal: 22.1872411173312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785394030090042;-9.95165603404587;-0.293078632378504]
Robot Arm 1 end: [-9.62242459887614;-0.29053598966238;-0.0304672792472796]
Robot Arm 1 togoal: 22.2112973239226
Mobile Platform 2 Pose: [-2.35619112376337;9.96187046114646;0.0767630811967251]
Robot Arm 2 end: [9.5855437624994;0.0897794169703806;-0.00533479258109709]
Robot Arm 1 togoal: 22.0845391859315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785393737431945;-9.94848939146609;-0.0687186671482844]
Robot Arm 1 end: [-9.63257477357302;-0.0662725817445094;-0.0367141352220022]
Robot Arm 1 togoal: 22.1177374353026
Mobile Platform 2 Pose: [-2.35619083521421;9.95870017276389;-0.161344133042812]
Robot Arm 2 end: [9.59559015693728;-0.148803433554656;-0.0127444987500212]
Robot Arm 1 togoal: 21.9860319197632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785393443620566;-9.94532443005105;0.15179608343956]
Robot Arm 1 end: [-9.6427151591788;0.154154350311994;-0.0426340082711946]
Robot Arm 1 togoal: 22.027752647375
Mobile Platform 2 Pose: [-2.35619054460882;9.95552838347223;-0.395306137176433]
Robot Arm 2 end: [9.60575440457244;-0.383236280248036;-0.0198662999858821]
Robot Arm 1 togoal: 21.8915969990401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785393146691449;-9.94216169739885;0.368487118176906]
Robot Arm 1 end: [-9.6528203505519;0.370766461326195;-0.0482246503337]
Robot Arm 1 togoal: 21.9412386222064
Mobile Platform 2 Pose: [-2.35430934770498;9.95235594259805;-0.625152519005053]
Robot Arm 2 end: [9.61600373461716;-0.613558502252943;-0.0266850429056848]
Robot Arm 1 togoal: 21.8011058320255

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785392850539007;-9.93900171181181;0.581376999361864]
Robot Arm 1 end: [-9.66286625239277;0.583586431996825;-0.05348594147317]
Robot Arm 1 togoal: 21.8580914016539
Mobile Platform 2 Pose: [-2.35222060646892;9.94918361046936;-0.850908637403783]
Robot Arm 2 end: [9.62630751334413;-0.839783499674608;-0.0331890039763766]
Robot Arm 1 togoal: 21.7144425763422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785392553188084;-9.93584491370056;0.790489328172648]
Robot Arm 1 end: [-9.67283015651397;0.792637945277728;-0.0584196902495051]
Robot Arm 1 togoal: 21.7782076611399
Mobile Platform 2 Pose: [-2.34996023375197;9.94601216191014;-1.07260644246374]
Robot Arm 2 end: [9.63663500036379;-1.06194176117798;-0.0393692031040723]
Robot Arm 1 togoal: 21.6314833349006

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785392250647586;-9.93269174060869;0.995848647057939]
Robot Arm 1 end: [-9.6826909164653;0.997945590803966;-0.0630294625057398]
Robot Arm 1 togoal: 21.7014850790751
Mobile Platform 2 Pose: [-2.34757002897787;9.94284227466063;-1.2902792926623]
Robot Arm 2 end: [9.64695654080532;-1.28006526705057;-0.0452194979436629]
Robot Arm 1 togoal: 21.552104458241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785391951137931;-9.92954256135376;1.19748035344503]
Robot Arm 1 end: [-9.69242893133302;1.19953478069377;-0.0673203785307385]
Robot Arm 1 togoal: 21.6278225190766
Mobile Platform 2 Pose: [-2.34509792807837;9.93967458435853;-1.50396165106515]
Robot Arm 2 end: [9.6572438441184;-1.49418719711343;-0.0507364162797272]
Robot Arm 1 togoal: 21.4761831419631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785391644069396;-9.92639771317755;1.39541072073196]
Robot Arm 1 end: [-9.70202625113044;1.39743177251666;-0.0712989255208994]
Robot Arm 1 togoal: 21.5571202709674
Mobile Platform 2 Pose: [-2.34259775586185;9.93650971069454;-1.71368914719078]
Robot Arm 2 end: [9.66747016041108;-1.70434200359625;-0.0559189565777115]
Robot Arm 1 togoal: 21.4035977699912

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785391340131286;-9.92325747951926;1.58966685889944]
Robot Arm 1 end: [-9.71146655697691;1.59166363259462;-0.0749727656456677]
Robot Arm 1 togoal: 21.4892801964825
Mobile Platform 2 Pose: [-2.34012866583489;9.93334816147643;-1.91949826184855]
Robot Arm 2 end: [9.6776103215344;-1.91056510657779;-0.0607683835327494]
Robot Arm 1 togoal: 21.3342282747752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785391035086397;-9.92012212139991;1.78027663722303]
Robot Arm 1 end: [-9.72073521558296;1.78225816091726;-0.0783505543693887]
Robot Arm 1 togoal: 21.4242059496427
Mobile Platform 2 Pose: [-2.33775412086585;9.93019043151043;-2.1214263661883]
Robot Arm 2 end: [9.68764094777477;-2.1128929434456;-0.0652879895169583]
Robot Arm 1 togoal: 21.2679564931785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.785390719937337;-9.91699186309072;1.96726870979174]
Robot Arm 1 end: [-9.72981922873829;1.96924391925737;-0.0814417582892766]
Robot Arm 1 togoal: 21.3618030516103
Mobile Platform 2 Pose: [-2.33554040742229;9.92703695542301;-2.31951167551583]
Robot Arm 2 end: [9.69754048450597;-2.31136293176498;-0.0694828645620831]
Robot Arm 1 togoal: 21.2046663932318

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785390408064061;-9.91386688713407;2.15067254747007]
Robot Arm 1 end: [-9.7387071866187;2.15265026608131;-0.0842565026278945]
Robot Arm 1 togoal: 21.3019789617006
Mobile Platform 2 Pose: [-2.33355462238774;9.92388811837952;-2.51379317517266]
Robot Arm 2 end: [9.70728925527756;-2.50601340180292;-0.0733596783841797]
Robot Arm 1 togoal: 21.1442443163428

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785390095087474;-9.91074731966555;2.33051838502194]
Robot Arm 1 end: [-9.74738923729095;2.33250730555084;-0.0868054041837619]
Robot Arm 1 togoal: 21.2446431866065
Mobile Platform 2 Pose: [-2.33186202881079;9.92074425025056;-2.70431056736513]
Robot Arm 2 end: [9.71686948143382;-2.69688354885921;-0.0769264483317184]
Robot Arm 1 togoal: 21.0865791673542

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785389802770181;-9.90763329645115;2.50683723447003]
Robot Arm 1 end: [-9.75585705499386;2.50884590404083;-0.0890994440749548]
Robot Arm 1 togoal: 21.1897073572659
Mobile Platform 2 Pose: [-2.33318034684256;9.91760565657478;-2.89110420067008]
Robot Arm 2 end: [9.72626562649469;-2.88400449020268;-0.0801923167591658]
Robot Arm 1 togoal: 21.0315659069345

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.78538950978426;-9.904524901869;2.67966092334582]
Robot Arm 1 end: [-9.76410373821774;2.68169772996344;-0.0911498215139012]
Robot Arm 1 togoal: 21.1370852233145
Mobile Platform 2 Pose: [-2.33449157715478;9.91447260178498;-3.07422395161766]
Robot Arm 2 end: [9.73546244572861;-3.06743718684554;-0.0831672161262374]
Robot Arm 1 togoal: 20.9790909612974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.785389216145724;-9.90142219684046;2.84902208078709]
Robot Arm 1 end: [-9.77212373767479;2.85109524281976;-0.0929678550309057]
Robot Arm 1 togoal: 21.0866926890451
Mobile Platform 2 Pose: [-2.33579449774701;9.91134527929343;-3.25371003017774]
Robot Arm 2 end: [9.74444798235579;-3.24722160705598;-0.0858619797371976]
Robot Arm 1 togoal: 20.9290527156413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.785388921869721;-9.89832521735964;3.01495415385275]
Robot Arm 1 end: [-9.77991277539206;3.0170717112317;-0.0945648681064362]
Robot Arm 1 togoal: 21.0384478213403
Mobile Platform 2 Pose: [-2.33708758518971;9.90822386546553;-3.42960302944722]
Robot Arm 2 end: [9.75321210493232;-3.42339816902905;-0.0882880163822395]
Robot Arm 1 togoal: 20.8813526387839

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785388626970602;-9.89523397649207;3.17749141337939]
Robot Arm 1 end: [-9.7874677818875;3.17966122086344;-0.0959521065061722]
Robot Arm 1 togoal: 20.9922708727799
Mobile Platform 2 Pose: [-2.33836893347783;9.90510846880325;-3.6019440234674]
Robot Arm 2 end: [9.76174631449726;-3.59600783281167;-0.0904571172454792]
Robot Arm 1 togoal: 20.835895159956

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785388331462009;-9.89214846642926;3.33666895950793]
Robot Arm 1 end: [-9.79478676771179;3.33889868173125;-0.0971406546023846]
Robot Arm 1 togoal: 20.9480842353065
Mobile Platform 2 Pose: [-2.33963612855043;9.90199919524549;-3.77077446246346]
Robot Arm 2 end: [9.7700437773646;-3.76509199288508;-0.0923812998057307]
Robot Arm 1 togoal: 20.7925878147637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785388035356957;-9.88906866043394;3.49252272332135]
Robot Arm 1 end: [-9.80186876340812;3.49481983148115;-0.0981413735015582]
Robot Arm 1 togoal: 20.9058124533708
Mobile Platform 2 Pose: [-2.34088610400426;9.89889609674773;-3.93613635068875]
Robot Arm 2 end: [9.77809914944145;-3.93069265025643;-0.0940726769387436]
Robot Arm 1 togoal: 20.7513410993743

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.78538773866792;-9.88599451474048;3.64508946505356]
Robot Arm 1 end: [-9.80871373122864;3.64746123509138;-0.0989648456002003]
Robot Arm 1 togoal: 20.8653822046569
Mobile Platform 2 Pose: [-2.34211495505145;9.89579920382501;-4.09807208340121]
Robot Arm 2 end: [9.78590851704457;-4.0928522449943;-0.0955433357586523]
Robot Arm 1 togoal: 20.7120685302244

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.785387441406916;-9.88292597037674;3.79440676811982]
Robot Arm 1 end: [-9.81532246979089;3.79686028017681;-0.0996213272063191]
Robot Arm 1 togoal: 20.8267222685579
Mobile Platform 2 Pose: [-2.3421385057904;9.89270852074839;-4.25662449905591]
Robot Arm 2 end: [9.7934691565519;-4.25161574517582;-0.0968052363219642]
Robot Arm 1 togoal: 20.6746858881859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.78583573832575;-9.87986295041141;3.94051303469183]
Robot Arm 1 end: [-9.82169654584163;3.94305562921349;-0.100120715436492]
Robot Arm 1 togoal: 20.7897633810549
Mobile Platform 2 Pose: [-2.34216120065351;9.88962401785132;-4.41183273883445]
Robot Arm 2 end: [9.80078012992728;-4.40701991603886;-0.097870139994114]
Robot Arm 1 togoal: 20.6391148187967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.78722148350574;-9.87680537628722;4.08344590569687]
Robot Arm 1 end: [-9.82783833899744;4.08608493741106;-0.100472514394302]
Robot Arm 1 togoal: 20.7544389873557
Mobile Platform 2 Pose: [-2.34218288880462;9.88654565607747;-4.56374072942616]
Robot Arm 2 end: [9.80784109752876;-4.559108866647;-0.0987495206218307]
Robot Arm 1 togoal: 20.6052773283813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.788849253442562;-9.87375313596342;4.22324206435016]
Robot Arm 1 end: [-9.83375085135759;4.22598379301911;-0.100685817763636]
Robot Arm 1 togoal: 20.720685338177
Mobile Platform 2 Pose: [-2.34220338555328;9.88347337630155;-4.71239278694141]
Robot Arm 2 end: [9.81465272041274;-4.7079271259016;-0.0994544938379794]
Robot Arm 1 togoal: 20.573098223212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.789707961076971;-9.87070614682073;4.35994154957372]
Robot Arm 1 end: [-9.83943744346935;4.36279141143463;-0.100769299338938]
Robot Arm 1 togoal: 20.6884396319444
Mobile Platform 2 Pose: [-2.34222245729886;9.88040710288106;-4.8578336917898]
Robot Arm 2 end: [9.82121656662338;-4.85351971269811;-0.0999957926027361]
Robot Arm 1 togoal: 20.5425050295286

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.789732302076024;-9.86766429244845;4.4935884737154]
Robot Arm 1 end: [-9.84490166976102;4.49655205028029;-0.100731217326778]
Robot Arm 1 togoal: 20.6576395979553
Mobile Platform 2 Pose: [-2.34223982402601;9.87734675474349;-5.00010862679529]
Robot Arm 2 end: [9.82753501098716;-4.99593207008383;-0.100383721333851]
Robot Arm 1 togoal: 20.5134279346711

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.789739022726428;-9.8646274412414;4.62422796589794]
Robot Arm 1 end: [-9.85014753043598;4.62731122459816;-0.100579391261509]
Robot Arm 1 togoal: 20.6282248722367
Mobile Platform 2 Pose: [-2.34225510999164;9.87429221972326;-5.13926326599677]
Robot Arm 2 end: [9.83361111511991;-5.13521015014588;-0.100628138302343]
Robot Arm 1 togoal: 20.4857996663051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.789744347835087;-9.86159549724063;4.75190285705727]
Robot Arm 1 end: [-9.85517957662082;4.75511159884826;-0.100321189531227]
Robot Arm 1 togoal: 20.6001381733235
Mobile Platform 2 Pose: [-2.34451382750524;9.87124337259561;-5.2753436163665]
Robot Arm 2 end: [9.83944872210059;-5.27139900631204;-0.100738434909968]
Robot Arm 1 togoal: 20.4595559215157

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.789748060642103;-9.85856831157315;4.87665655595075]
Robot Arm 1 end: [-9.86000258629855;4.879996412211;-0.0999635674747149]
Robot Arm 1 togoal: 20.5733240472666
Mobile Platform 2 Pose: [-2.34655630604118;9.86820012147371;-5.4084042198073]
Robot Arm 2 end: [9.84505130827661;-5.40455544478938;-0.100723556089349]
Robot Arm 1 togoal: 20.4346313737371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.789750026978517;-9.85554576406897;4.99853297480146]
Robot Arm 1 end: [-9.86462160457805;5.0020094129635;-0.0995130672876961]
Robot Arm 1 togoal: 20.5477289256605
Mobile Platform 2 Pose: [-2.34834243707354;9.86516232091797;-5.53849089058805]
Robot Arm 2 end: [9.85042365405134;-5.5347254810978;-0.100591915071596]
Robot Arm 1 togoal: 20.4109665649009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.789750293875333;-9.85252772358977;5.11757664608797]
Robot Arm 1 end: [-9.8690418425698;5.12119497510564;-0.098975823218777]
Robot Arm 1 togoal: 20.5233009986341
Mobile Platform 2 Pose: [-2.34984036140937;9.86212983495744;-5.66564964242134]
Robot Arm 2 end: [9.85557093167376;-5.66195531957682;-0.100351456313895]
Robot Arm 1 togoal: 20.3885043398717

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.789749015248258;-9.84951404178476;5.23383243330288]
Robot Arm 1 end: [-9.87326862531425;5.23759780911538;-0.0983575745863902]
Robot Arm 1 togoal: 20.4999902301443
Mobile Platform 2 Pose: [-2.35102605937418;9.8591025057561;-5.78992662563394]
Robot Arm 2 end: [9.86049863533171;-5.78629129604593;-0.10000965909248]
Robot Arm 1 togoal: 20.3671897767135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.789746414874387;-9.84650460765898;5.34734567132239]
Robot Arm 1 end: [-9.87730741602461;5.35126310267214;-0.0976636875727695]
Robot Arm 1 togoal: 20.4777483377736
Mobile Platform 2 Pose: [-2.35188272483597;9.856080204545;-5.91136814169791]
Robot Arm 2 end: [9.86521252689336;-5.90777989627118;-0.0995735267929898]
Robot Arm 1 togoal: 20.3469701103566

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.789742695075737;-9.84349927350032;5.45816206650891]
Robot Arm 1 end: [-9.88116370354014;5.46223641999829;-0.0968991511463698]
Robot Arm 1 togoal: 20.4565286932615
Mobile Platform 2 Pose: [-2.35240002405472;9.85306277707963;-6.03002056949107]
Robot Arm 2 end: [9.86971855662207;-6.02646768600249;-0.0990496036437203]
Robot Arm 1 togoal: 20.3277946489042

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.789738058565999;-9.84049789010121;5.56632759932134]
Robot Arm 1 end: [-9.88484301925421;5.57056360531482;-0.0960686130471188]
Robot Arm 1 togoal: 20.4362863449856
Mobile Platform 2 Pose: [-2.35257330356949;9.8500500446947;-6.14593041398554]
Robot Arm 2 end: [9.8740227793314;-6.1424013614775;-0.0984439877881103]
Robot Arm 1 togoal: 20.3096146570755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.789732690730978;-9.83750036373541;5.67188855409726]
Robot Arm 1 end: [-9.88835092878415;5.67629081182772;-0.0951763861008417]
Robot Arm 1 togoal: 20.4169779820948
Mobile Platform 2 Pose: [-2.35240279887271;9.84704186837561;-6.25914429101112]
Robot Arm 2 end: [9.8781313940128;-6.2556277358848;-0.0977623562264106]
Robot Arm 1 togoal: 20.2923833724836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.789726714115977;-9.83450650424642;5.77489128052018]
Robot Arm 1 end: [-9.89169290341627;5.77946426354955;-0.0942264651359798]
Robot Arm 1 togoal: 20.3985618365647
Mobile Platform 2 Pose: [-2.35189313091954;9.84403807175477;-6.36970878797344]
Robot Arm 2 end: [9.88205061783967;-6.3661936017611;-0.0970099657113258]
Robot Arm 1 togoal: 20.2760558777925

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.789720243545802;-9.83151620895007;5.87538243383028]
Robot Arm 1 end: [-9.89487444245642;5.88013049531184;-0.093222551903455]
Robot Arm 1 togoal: 20.3809977249358
Mobile Platform 2 Pose: [-2.35105296633727;9.84103850887323;-6.47767053136834]
Robot Arm 2 end: [9.88578670371074;-6.47414579826709;-0.0961916906343527]
Robot Arm 1 togoal: 20.2605890743979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.789713393786378;-9.82852936096898;5.9734086249777]
Robot Arm 1 end: [-9.89790097339431;5.97833600243947;-0.0921680602513681]
Robot Arm 1 togoal: 20.3642469943501
Mobile Platform 2 Pose: [-2.34989525086697;9.83804303389967;-6.58307629876604]
Robot Arm 2 end: [9.88934589426922;-6.57953132493287;-0.0953120318592378]
Robot Arm 1 togoal: 20.2459415845085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.78970618651716;-9.82554583291111;6.06901653406449]
Robot Arm 1 end: [-9.90077783741368;6.07412735451238;-0.0910661417609255]
Robot Arm 1 togoal: 20.3482724526702
Mobile Platform 2 Pose: [-2.3462801836169;9.83505148432622;-6.68597262115923]
Robot Arm 2 end: [9.89273423685686;-6.68239477306697;-0.0943751396301899]
Robot Arm 1 togoal: 20.2320739594765

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.789698721967279;-9.82256547025882;6.1622528468599]
Robot Arm 1 end: [-9.90351026185113;6.16755113145313;-0.089919699790076]
Robot Arm 1 togoal: 20.3330383207432
Mobile Platform 2 Pose: [-2.34305554463236;9.83206371764411;-6.78639777469033]
Robot Arm 2 end: [9.89595855080227;-6.78277718357839;-0.0933847380443482]
Robot Arm 1 togoal: 20.2189497103133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.789691019907705;-9.81958816556297;6.25316416176648]
Robot Arm 1 end: [-9.90610340311475;6.25865382992183;-0.0887313987609181]
Robot Arm 1 togoal: 20.3185102563158
Mobile Platform 2 Pose: [-2.34034110641509;9.8290795457944;-6.88440084269569]
Robot Arm 2 end: [9.89902464413189;-6.8807282195439;-0.0923443905009724]
Robot Arm 1 togoal: 20.2065315764601

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.789683112961864;-9.81661379760142;6.34179690191892]
Robot Arm 1 end: [-9.90856228624965;6.34748177593181;-0.0875036951181613]
Robot Arm 1 togoal: 20.3046552745877
Mobile Platform 2 Pose: [-2.33825112472061;9.82609885774099;-6.98003129303831]
Robot Arm 2 end: [9.90193828588388;-6.9762979505926;-0.0912573501486367]
Robot Arm 1 togoal: 20.1947838656874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.789675055753864;-9.81364224916882;6.42819735037505]
Robot Arm 1 end: [-9.9108918309401;6.43408115910404;-0.0862388318133393]
Robot Arm 1 togoal: 20.2914417266429
Mobile Platform 2 Pose: [-2.33688508885863;9.82312149788589;-7.07333856561653]
Robot Arm 2 end: [9.90470505752531;-7.06953642033383;-0.0901265983959836]
Robot Arm 1 togoal: 20.1836723401586

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.789666865817521;-9.81067340722156;6.51241160351513]
Robot Arm 1 end: [-9.91309681889403;6.51849798639852;-0.0849388701156125]
Robot Arm 1 togoal: 20.2788392380803
Mobile Platform 2 Pose: [-2.33631771221556;9.82014739057152;-7.16437242201554]
Robot Arm 2 end: [9.90733048630702;-7.16049397129322;-0.0889548665891699]
Robot Arm 1 togoal: 20.1731642597428

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.789666557650298;-9.80770715832893;6.59448539366562]
Robot Arm 1 end: [-9.91518190756522;6.60077788913587;-0.0836056890483494]
Robot Arm 1 togoal: 20.2668187140391
Mobile Platform 2 Pose: [-2.33626727086572;9.8171763294718;-7.25318242032646]
Robot Arm 2 end: [9.90981980780774;-7.24922017032431;-0.0877446539082232]
Robot Arm 1 togoal: 20.1632282586942

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.789666249258737;-9.8047433957216;6.67446421993877]
Robot Arm 1 end: [-9.91715161724226;6.68096629768638;-0.0822410086703669]
Robot Arm 1 togoal: 20.2553522554705
Mobile Platform 2 Pose: [-2.3362166921084;9.81420820007131;-7.33981654814467]
Robot Arm 2 end: [9.91217826207073;-7.33576249490301;-0.0864982306002024]
Robot Arm 1 togoal: 20.1538347513676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.789665940661942;-9.80178201476896;6.75239323512111]
Robot Arm 1 end: [-9.91901032569613;6.75910828557898;-0.0808463995904703]
Robot Arm 1 togoal: 20.2444131391274
Mobile Platform 2 Pose: [-2.33616599348539;9.81124291027331;-7.42432038110138]
Robot Arm 2 end: [9.91441112819842;-7.42016675747464;-0.0852176330512561]
Robot Arm 1 togoal: 20.1449558258855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.789665631879544;-9.79882291416647;6.82831721265828]
Robot Arm 1 end: [-9.92076227155312;6.83524854966996;-0.079423287570722]
Robot Arm 1 togoal: 20.2339757813807
Mobile Platform 2 Pose: [-2.33611522567645;9.8082802775762;-7.50673873850953]
Robot Arm 2 end: [9.91652342688054;-7.50247800517942;-0.0839047246294093]
Robot Arm 1 togoal: 20.1365647691763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.789665322931693;-9.79586599582883;6.90228051045403]
Robot Arm 1 end: [-9.92241154416808;6.90943137391723;-0.0779729714865499]
Robot Arm 1 togoal: 20.2240156925195
Mobile Platform 2 Pose: [-2.33606439885047;9.80532023611986;-7.58711626200756]
Robot Arm 2 end: [9.91852014189892;-7.58274109942224;-0.082561194220316]
Robot Arm 1 togoal: 20.1286361619277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.789665013839058;-9.79291116489083;6.97432703900593]
Robot Arm 1 end: [-9.92396209775981;6.98170059781676;-0.0764966370790325]
Robot Arm 1 togoal: 20.214509453847
Mobile Platform 2 Pose: [-2.33601352460095;9.80236261709112;-7.66549700456294]
Robot Arm 2 end: [9.9204059829214;-7.66100030519133;-0.0811885666190926]
Robot Arm 1 togoal: 20.1211456507757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.789664704622825;-9.78995832987434;7.04450023434103]
Robot Arm 1 end: [-9.92541774557924;7.05209958819174;-0.0749953513636325]
Robot Arm 1 togoal: 20.2054346732595
Mobile Platform 2 Pose: [-2.33596262728576;9.7994073751064;-7.74192431417793]
Robot Arm 2 end: [9.92218560224954;-7.73729917410938;-0.0797882299858253]
Robot Arm 1 togoal: 20.1140701290223

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.78966439530469;-9.78700740243619;7.11284302753299]
Robot Arm 1 end: [-9.92678215369545;7.12067120913691;-0.0734700858558823]
Robot Arm 1 togoal: 20.1967699432711
Mobile Platform 2 Pose: [-2.33591169497166;9.79645438534513;-7.81644113457524]
Robot Arm 2 end: [9.92386341614457;-7.81168084651797;-0.0783614272108322]
Robot Arm 1 togoal: 20.1073874761872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.789664085906851;-9.784058297415;7.17939782101314]
Robot Arm 1 end: [-9.92805885152762;7.18745779809235;-0.0719217202036763]
Robot Arm 1 togoal: 20.1884948134861
Mobile Platform 2 Pose: [-2.33586072912775;9.79350350549617;-7.88908980141377]
Robot Arm 2 end: [9.92544364417306;-7.88418784728849;-0.0769092787249432]
Robot Arm 1 togoal: 20.1010765767835

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.789663776452006;-9.7811109328132;7.24420646627318]
Robot Arm 1 end: [-9.92925122106642;7.25250114308089;-0.0703510503165527]
Robot Arm 1 togoal: 20.1805897427824
Mobile Platform 2 Pose: [-2.33580977901812;9.79055466759796;-7.95991188588606]
Robot Arm 2 end: [9.92693041889807;-7.95486192965687;-0.0754327949022843]
Robot Arm 1 togoal: 20.0951173998813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.78966346696334;-9.77816522969433;7.30731024277772]
Robot Arm 1 end: [-9.93036252832461;7.31584246191424;-0.068758791154336]
Robot Arm 1 togoal: 20.1730360931061
Mobile Platform 2 Pose: [-2.33575880935387;9.78760775448633;-8.02894846906038]
Robot Arm 2 end: [9.92832764786101;-8.02374435001308;-0.0739328775323807]
Robot Arm 1 togoal: 20.0894907879329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.789663157464525;-9.77522111216946;7.36874984027462]
Robot Arm 1 end: [-9.93139588924372;7.37752238396703;-0.0671455924760772]
Robot Arm 1 togoal: 20.1658160599381
Mobile Platform 2 Pose: [-2.33570782063456;9.78466267315674;-8.09624001197277]
Robot Arm 2 end: [9.92963911439364;-8.09087573839712;-0.0724103313066753]
Robot Arm 1 togoal: 20.0841785283871

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.791512298352881;-9.77227849443714;7.42856531011248]
Robot Arm 1 end: [-9.93235457318901;7.43757573427745;-0.0655120361691161]
Robot Arm 1 togoal: 20.1589135964665
Mobile Platform 2 Pose: [-2.33565680022032;9.78171931008958;-8.16182625696602]
Robot Arm 2 end: [9.93086840006282;-8.15629599952924;-0.0708658799553656]
Robot Arm 1 togoal: 20.0791632445613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.795457849334523;-9.76933734762805;7.48678874577598]
Robot Arm 1 end: [-9.93324188278305;7.49603356728562;-0.063858522714459]
Robot Arm 1 togoal: 20.1523139215009
Mobile Platform 2 Pose: [-2.33560578090869;9.77877758819402;-8.22574631363735]
Robot Arm 2 end: [9.93201897702727;-8.22004439984234;-0.0693001632809184]
Robot Arm 1 togoal: 20.0744284364073

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.799173242792004;-9.76639752473876;7.54345040655082]
Robot Arm 1 end: [-9.93406046698356;7.55293260410499;-0.0621854057893726]
Robot Arm 1 togoal: 20.1460016273829
Mobile Platform 2 Pose: [-2.33555472817448;9.77583745949716;-8.28803866244129]
Robot Arm 2 end: [9.93309418532312;-8.28215957082573;-0.0677137489694932]
Robot Arm 1 togoal: 20.0699584184231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.802538549977962;-9.76345894362811;7.59858985832918]
Robot Arm 1 end: [-9.93481281969768;7.60831277032245;-0.0604931549377655]
Robot Arm 1 togoal: 20.1399615332329
Mobile Platform 2 Pose: [-2.33550366178174;9.77289881044523;-8.34874105240559]
Robot Arm 2 end: [9.93409716259136;-8.34267940706146;-0.0661071417571161]
Robot Arm 1 togoal: 20.0657382200314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.805439653315791;-9.76052165955534;7.65224637427976]
Robot Arm 1 end: [-9.93550141940431;7.66221370036161;-0.0587821798913949]
Robot Arm 1 togoal: 20.1341792323837
Mobile Platform 2 Pose: [-2.33545257430648;9.76996156653625;-8.40789035632902]
Robot Arm 2 end: [9.93503092098404;-8.40164092178599;-0.0644807873655999]
Robot Arm 1 togoal: 20.0617536344808

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.807755185480001;-9.75758552951345;7.70445877389635]
Robot Arm 1 end: [-9.93612841395844;7.71467461562754;-0.0570528534297415]
Robot Arm 1 togoal: 20.1286407653005
Mobile Platform 2 Pose: [-2.33540147835863;9.76702566306295;-8.46552312151257]
Robot Arm 2 end: [9.93589835057569;-8.4590807981601;-0.0628350743169786]
Robot Arm 1 togoal: 20.057991140035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.809370845938789;-9.75465051837535;7.75526621869062]
Robot Arm 1 end: [-9.93669594144002;7.76573507503722;-0.0553055128655977]
Robot Arm 1 togoal: 20.1233328225282
Mobile Platform 2 Pose: [-2.33535031691194;9.76409103606775;-8.52167489474163]
Robot Arm 2 end: [9.93670219845918;-8.51503471385761;-0.0611703461911401]
Robot Arm 1 togoal: 20.0544378992646

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.810191165511135;-9.75171658858888;7.80470702342004]
Robot Arm 1 end: [-9.93720602004082;7.81543374534601;-0.0535404737458405]
Robot Arm 1 togoal: 20.1182427490571
Mobile Platform 2 Pose: [-2.33795682568988;9.76115759896583;-8.57638075558291]
Robot Arm 2 end: [9.93744538326447;-8.56954605276557;-0.0594868981402699]
Robot Arm 1 togoal: 20.051081410762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.810152311789763;-9.74878365721337;7.85281957813665]
Robot Arm 1 end: [-9.93766051107196;7.86380927416857;-0.0517580205747242]
Robot Arm 1 togoal: 20.1133583785296
Mobile Platform 2 Pose: [-2.34050090854164;9.75822533715983;-8.62968627395486]
Robot Arm 2 end: [9.93812960726646;-8.62265233568238;-0.0577851886299415]
Robot Arm 1 togoal: 20.047909155231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.809233374166484;-9.74585173926872;7.89964168072135]
Robot Arm 1 end: [-9.9380612970673;7.91089957779605;-0.0499584184953438]
Robot Arm 1 togoal: 20.1086682605365
Mobile Platform 2 Pose: [-2.34291972048407;9.75529410888908;-8.68162455855673]
Robot Arm 2 end: [9.93875718756327;-8.67438657013091;-0.0560654095230664]
Robot Arm 1 togoal: 20.044910188274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.807462594214223;-9.74292073524481;7.94521053415962]
Robot Arm 1 end: [-9.93841004991541;7.9567418055566;-0.0481419102207153]
Robot Arm 1 togoal: 20.1041614026086
Mobile Platform 2 Pose: [-2.34515825979347;9.7523639229473;-8.73222777126289]
Robot Arm 2 end: [9.93933048953439;-8.72478082687848;-0.0543277280790465]
Robot Arm 1 togoal: 20.0420742878881

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.804915994382714;-9.73999069909171;7.98956240098403]
Robot Arm 1 end: [-9.93870854500658;8.00137198895417;-0.0463087099586996]
Robot Arm 1 togoal: 20.0998275853692
Mobile Platform 2 Pose: [-2.34716992588328;9.74943471268053;-8.7815269625723]
Robot Arm 2 end: [9.93985171400267;-8.77386607990757;-0.0525722895264841]
Robot Arm 1 togoal: 20.039391720229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.801708706866255;-9.73706151475925;8.03273230962455]
Robot Arm 1 end: [-9.93895831427813;8.04482476877537;-0.0444590183551077]
Robot Arm 1 togoal: 20.0956570129335
Mobile Platform 2 Pose: [-2.34891644013738;9.74650641843678;-8.82955263398359]
Robot Arm 2 end: [9.94032294871205;-8.82167277232575;-0.0507992131408174]
Robot Arm 1 togoal: 20.036853224018

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.797981858013078;-9.7341330830859;8.07475441651991]
Robot Arm 1 end: [-9.93916087269909;8.08713379658329;-0.0425929992202658]
Robot Arm 1 togoal: 20.0916404628084
Mobile Platform 2 Pose: [-2.35036749747594;9.74357897334212;-8.87633406356223]
Robot Arm 2 end: [9.94074617038994;-8.86823014324611;-0.0490085950138945]
Robot Arm 1 togoal: 20.0344500256183
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('pinv')" style="font-weight:bold">pinv</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 22)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',22,0)">第 22 行</a>)
    X = pinv(mobile_robot.J)*s_d;

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
    [dx1] = mobile_robot_CBLF(mobile_robot1, target1, a1_G, obstacle_results1, D_min, V_max, dt);
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',23,1)">23  </a>    dx = fmincon(fun1, X, [], [], [], [], [], [], fun2, options);
K_s

K_s =

    0.0354

d

d =

   28.2843

s

s =

         0
         0
   -0.0548
   20.5330
   19.4369
         0

K_s*s

ans =

         0
         0
   -0.0019
    0.7260
    0.6872
         0

X

X =

         0
         0
   -0.0019
    0.7260
    0.6872
         0

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',23,1)">23  </a>    dx = fmincon(fun1, X, [], [], [], [], [], [], fun2, options);
X

X =

   -6.8157
    6.9283
   -6.8157
    6.9283
   -0.0005
    0.9004
   -0.0005
   -0.0000
   -0.0005
   -0.9004
   -0.0005

s_k

s_k =

         0
         0
   -0.0019
    0.7260
    0.6872
         0

mobile_robot.car.J*X(1:4)

ans =

   -0.0000
    0.0056
    0.6872

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.775441e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785398045164464;-9.99859311732627;-9.82820014583124]
Robot Arm 1 end: [-9.85431393467722;-9.82623536718414;1.50722029826549]
Robot Arm 1 togoal: 28.0583959009043
Mobile Platform 2 Pose: [-2.35629151108197;9.99866821765336;9.81851213362279]
Robot Arm 2 end: [9.85352518005178;9.82447106417814;1.50725304565207]
Robot Arm 1 togoal: 28.056591097782

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.244993e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785397927092379;-9.99718522068336;-9.65642237306685]
Robot Arm 1 end: [-9.71362915021666;-9.65258678365133;1.46633502494473]
Robot Arm 1 togoal: 27.8362053775423
Mobile Platform 2 Pose: [-2.35629139293979;9.99733410827729;9.63723046804381]
Robot Arm 2 end: [9.71208515271964;9.64891502617636;1.46646969259345]
Robot Arm 1 togoal: 27.8325194031801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.905433e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785397809283982;-9.99577693302966;-9.48468960484044]
Robot Arm 1 end: [-9.5828283004008;-9.47914174047015;1.39971729151173]
Robot Arm 1 togoal: 27.6213474813175
Mobile Platform 2 Pose: [-2.35629127487691;9.99599782273485;9.45617822091655]
Robot Arm 2 end: [9.58056398002722;9.47315666743041;1.40002691168591]
Robot Arm 1 togoal: 27.6155201084159

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.723210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785397691804528;-9.99436845073056;-9.31305341171688]
Robot Arm 1 end: [-9.46646721632904;-9.30601043144053;1.30962241201359]
Robot Arm 1 togoal: 27.417331473869
Mobile Platform 2 Pose: [-2.35629115684131;9.99465920267761;9.27539849372063]
Robot Arm 2 end: [9.46351493778736;9.29705577405621;1.31018137663547]
Robot Arm 1 togoal: 27.4089259142431

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.673057e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785397574685745;-9.99295958519151;-9.14159299845042]
Robot Arm 1 end: [-9.36861670170663;-9.13332273081556;1.19911162877552]
Robot Arm 1 togoal: 27.2273936281588
Mobile Platform 2 Pose: [-2.35629103874885;9.993317803263;9.09495413820532]
Robot Arm 2 end: [9.36500335289032;9.12051327954836;1.19999290699818]
Robot Arm 1 togoal: 27.2158121040802

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.737442e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785397457933299;-9.99154983651131;-8.97041284989568]
Robot Arm 1 end: [-9.29272021223066;-8.96122410469226;1.07194807886991]
Robot Arm 1 togoal: 27.0543660129682
Mobile Platform 2 Pose: [-2.35629092049066;9.99197293760633;8.91492665769692]
Robot Arm 2 end: [9.28846569159801;8.94347571905808;1.07322023958358]
Robot Arm 1 togoal: 27.0388736408959

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785397341537079;-9.99013849225644;-8.79963904815961]
Robot Arm 1 end: [-9.24147249192139;-8.78987053040305;0.932468543652386]
Robot Arm 1 togoal: 26.9005551519255
Mobile Platform 2 Pose: [-2.35629080194251;9.99062373869924;8.73541422973643]
Robot Arm 2 end: [9.23658981489445;8.7659378899552;0.934192674052159]
Robot Arm 1 togoal: 26.8803116692114

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785397225483446;-9.98872474348182;-8.62941436237]
Robot Arm 1 end: [-9.2167235447915;-8.61942257465971;0.785435357059431]
Robot Arm 1 togoal: 26.7676374780188
Mobile Platform 2 Pose: [-2.35629068297609;9.98926923573648;8.55652885834424]
Robot Arm 2 end: [9.21122059031023;8.58794356688544;0.787661979017709]
Robot Arm 1 togoal: 26.7417356816699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785397109768435;-9.98730780976734;-8.45989231012795]
Robot Arm 1 end: [-9.21941144185553;-8.45003886520389;0.635873528300053]
Robot Arm 1 togoal: 26.6565781594293
Mobile Platform 2 Pose: [-2.35629056347111;9.9879084414254;8.37839273954501]
Robot Arm 2 end: [9.21329417746829;8.40958423149757;0.638639631508669]
Robot Arm 1 togoal: 26.6240872997822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785396994410801;-9.98588706493589;-8.29123048570078]
Robot Arm 1 end: [-9.23734481364931;-8.28169954780987;0.492851444414927]
Robot Arm 1 togoal: 26.5584838288631
Mobile Platform 2 Pose: [-2.35629044332791;9.98654044649451;8.20113400087984]
Robot Arm 2 end: [9.23848874460507;8.23117385903496;0.493390793776496]
Robot Arm 1 togoal: 26.5245382643199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785396874948964;-9.98444565032734;-8.12371947843981]
Robot Arm 1 end: [-9.29237045600079;-8.11498872094174;0.354586548380931]
Robot Arm 1 togoal: 26.4897884950321
Mobile Platform 2 Pose: [-2.3561511814358;9.98516096788974;8.02484398502342]
Robot Arm 2 end: [9.29288233610898;8.05256082080095;0.355222041593133]
Robot Arm 1 togoal: 26.4474819079619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785396754957487;-9.98299572173646;-7.95739415790604]
Robot Arm 1 end: [-9.31982896935378;-7.94907779256683;0.282032297551359]
Robot Arm 1 togoal: 26.4000042798559
Mobile Platform 2 Pose: [-2.3560250991957;9.98377127880701;7.84967232993298]
Robot Arm 2 end: [9.33034214348762;7.87575982111798;0.285922972145128]
Robot Arm 1 togoal: 26.3577376083632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785396631520917;-9.9815401871915;-7.79222960511718]
Robot Arm 1 end: [-9.3286134178131;-7.78410224401873;0.260644727130959]
Robot Arm 1 togoal: 26.2955905941438
Mobile Platform 2 Pose: [-2.35552736602411;9.9823717817191;7.67554250141049]
Robot Arm 2 end: [9.33141190070732;7.70148076018264;0.264048697546498]
Robot Arm 1 togoal: 26.2416812295757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785396507444565;-9.98008404968554;-7.62799129163596]
Robot Arm 1 end: [-9.33035905927736;-7.61996806663369;0.254295748565721]
Robot Arm 1 togoal: 26.1864582502174
Mobile Platform 2 Pose: [-2.3551445113212;9.98096883263572;7.50226969737135]
Robot Arm 2 end: [9.33089391626946;7.52810818565404;0.257150684069728]
Robot Arm 1 togoal: 26.1249947610221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785396383027598;-9.97862438856213;-7.46462828970318]
Robot Arm 1 end: [-9.32994343908887;-7.45668355726409;0.252301130859057]
Robot Arm 1 togoal: 26.0766583556519
Mobile Platform 2 Pose: [-2.35459196088283;9.97956384534019;7.32988371499797]
Robot Arm 2 end: [9.32979547681143;7.35563918946027;0.255023100648243]
Robot Arm 1 togoal: 26.0088812208403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785396258314775;-9.9771599417069;-7.30213133960947]
Robot Arm 1 end: [-9.32889671793134;-7.29425784407421;0.251562673199429]
Robot Arm 1 togoal: 25.9674618289207
Mobile Platform 2 Pose: [-2.35374494766076;9.97815370512496;7.15839319316489]
Robot Arm 2 end: [9.3285848097976;7.18406514014771;0.254236193904151]
Robot Arm 1 togoal: 25.8938405036953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.78539613331805;-9.97569030407191;-7.14050215785427]
Robot Arm 1 end: [-9.32766609143492;-7.13269794233147;0.25118602938523]
Robot Arm 1 togoal: 25.8592428900218
Mobile Platform 2 Pose: [-2.35250666731562;9.97673722053766;6.98780609346032]
Robot Arm 2 end: [9.32736496560991;7.01338966889306;0.253809246613807]
Robot Arm 1 togoal: 25.7799968375667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785396008043131;-9.97421536048429;-6.9797454830995]
Robot Arm 1 end: [-9.32637938567155;-6.97201008043972;0.250913717943986]
Robot Arm 1 togoal: 25.7521103114546
Mobile Platform 2 Pose: [-2.35233503034862;9.97531398328107;6.81813054926681]
Robot Arm 2 end: [9.3754413349839;6.84170019533249;0.191703354693912]
Robot Arm 1 togoal: 25.7063731779901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785395882494139;-9.97273508632899;-6.81986685583477]
Robot Arm 1 end: [-9.32507280201445;-6.81220021222085;0.250671791991542]
Robot Arm 1 togoal: 25.6460961655964
Mobile Platform 2 Pose: [-2.35032873704384;9.97388251524153;6.64958881192911]
Robot Arm 2 end: [9.34218351741143;6.67432293338985;0.229322399029441]
Robot Arm 1 togoal: 25.5699342887901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785395756674772;-9.97124948511572;-6.66087198175696]
Robot Arm 1 end: [-9.32375652815973;-6.65327415606972;0.250439080831002]
Robot Arm 1 togoal: 25.5412104553824
Mobile Platform 2 Pose: [-2.35019239521884;9.97244612586693;6.48183140712126]
Robot Arm 2 end: [9.40952623272916;6.50381105064425;0.15256090775179]
Robot Arm 1 togoal: 25.5142727375005

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.78539563058865;-9.96975856950155;-6.50276654622366]
Robot Arm 1 end: [-9.32243340926272;-6.49523762666444;0.25020955833235]
Robot Arm 1 togoal: 25.4374567431996
Mobile Platform 2 Pose: [-2.34783741711322;9.97099991257639;6.31530527505113]
Robot Arm 2 end: [9.37034183973302;6.33868380836182;0.18743159107777]
Robot Arm 1 togoal: 25.3759952766194

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.78539550423942;-9.96826235557159;-6.34555615825393]
Robot Arm 1 end: [-9.32110422842073;-6.33809623935498;0.249981534889257]
Robot Arm 1 togoal: 25.334836594511
Mobile Platform 2 Pose: [-2.34543278614871;9.9695565808309;6.14955001855977]
Robot Arm 2 end: [9.3397648394546;6.17396386191076;0.224673865549105]
Robot Arm 1 togoal: 25.2448821050656

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.785395377630797;-9.96676086111866;-6.18924633177223]
Robot Arm 1 end: [-9.31976919299968;-6.18185550808817;0.249754559708286]
Robot Arm 1 togoal: 25.2333508342607
Mobile Platform 2 Pose: [-2.34543265918762;9.96810277524291;5.98461474881634]
Robot Arm 2 end: [9.32530616264152;6.00946127131466;0.243815322581151]
Robot Arm 1 togoal: 25.1277040172354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785395250766564;-9.96525410512466;-6.03384247755459]
Robot Arm 1 end: [-9.31842835103236;-6.02652084193233;0.249528534913988]
Robot Arm 1 togoal: 25.1329998992836
Mobile Platform 2 Pose: [-2.34543253219418;9.96663949126692;5.82057512009752]
Robot Arm 2 end: [9.32027078243187;5.84549104181109;0.249333846264847]
Robot Arm 1 togoal: 25.0193841704619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785395123650581;-9.96374210760237;-5.87934989826076]
Robot Arm 1 end: [-9.3170817072768;-5.87209754135724;0.249303462787741]
Robot Arm 1 togoal: 25.0337839355285
Mobile Platform 2 Pose: [-2.34543240500584;9.96516868520135;5.65747829473983]
Robot Arm 2 end: [9.31790507588767;5.68236075242616;0.250762441798022]
Robot Arm 1 togoal: 24.9144817073226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785394996286784;-9.96222488954472;-5.72577378437586]
Robot Arm 1 end: [-9.3157292553829;-5.71859079451341;0.249079373821627]
Robot Arm 1 togoal: 24.9357028237007
Mobile Platform 2 Pose: [-2.34539170154214;9.963690904723;5.49534204814299]
Robot Arm 2 end: [9.38869660407078;5.51738212991662;0.165557524740974]
Robot Arm 1 togoal: 24.8706238054918

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785394868679179;-9.96070247290571;-5.5731192104448]
Robot Arm 1 end: [-9.31437098675811;-5.5660056735575;0.248856306279498]
Robot Arm 1 togoal: 24.8387561850593
Mobile Platform 2 Pose: [-2.34313802321593;9.96220319122438;5.33448043607425]
Robot Arm 2 end: [9.35209331210982;5.35779185863488;0.200031009689838]
Robot Arm 1 togoal: 24.740861856595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.78539474083185;-9.95917488059025;-5.42139113142852]
Robot Arm 1 end: [-9.31300689300322;-5.41434713103324;0.248634300389164]
Robot Arm 1 togoal: 24.7429433818761
Mobile Platform 2 Pose: [-2.34102410993796;9.96071651430466;5.17444528906532]
Robot Arm 2 end: [9.32421878292795;5.19866653170457;0.235219536845043]
Robot Arm 1 togoal: 24.6186540515897

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785394612748948;-9.95764213644594;-5.27059437913785]
Robot Arm 1 end: [-9.31163696658709;-5.26361999631255;0.248413396677732]
Robot Arm 1 togoal: 24.6482635166058
Mobile Platform 2 Pose: [-2.34090734935316;9.95921977536783;5.01528738675089]
Robot Arm 2 end: [9.37954932288822;5.03728244939133;0.169221735457223]
Robot Arm 1 togoal: 24.5665239685536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785394484434696;-9.95610426525487;-5.12073365873683]
Robot Arm 1 end: [-9.31026120104327;-5.11382897210147;0.248193635476103]
Robot Arm 1 togoal: 24.5547154308907
Mobile Platform 2 Pose: [-2.33919484916307;9.9577142198495;4.85735219743578]
Robot Arm 2 end: [9.34474890015847;4.88053716484132;0.203400138475829]
Robot Arm 1 togoal: 24.4414763408329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785394355893386;-9.95456129272472;-4.97181354531743]
Robot Arm 1 end: [-9.30887959103759;-4.96497863101634;0.247975056752214]
Robot Arm 1 togoal: 24.4622977047148
Mobile Platform 2 Pose: [-2.33919471978865;9.95620771852821;4.70027223864339]
Robot Arm 2 end: [9.31933891348866;4.72428530779399;0.235721881357463]
Robot Arm 1 togoal: 24.3247481738623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785394227129377;-9.95301324547906;-4.8238384805503]
Robot Arm 1 end: [-9.3074921324011;-4.81707341223577;0.24775770003644]
Robot Arm 1 togoal: 24.3710086557929
Mobile Platform 2 Pose: [-2.33919459080411;9.95469114430684;4.54409702398085]
Robot Arm 2 end: [9.31121454265298;4.56828884013581;0.245342418806251]
Robot Arm 1 togoal: 24.2236526788685

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785394098147092;-9.95146015104681;-4.67681276941705]
Robot Arm 1 end: [-9.30609882215318;-4.67011761823407;0.247541604372904]
Robot Arm 1 togoal: 24.280846339223
Mobile Platform 2 Pose: [-2.3391944617862;9.95316706989251;4.38890253066352]
Robot Arm 2 end: [9.30790506859834;4.41309249217188;0.248015625201454]
Robot Arm 1 togoal: 24.1278453771651

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785393968951021;-9.9499020378507;-4.53074057702958]
Robot Arm 1 end: [-9.3046996585214;-4.52411541160166;0.247326808278267]
Robot Arm 1 togoal: 24.1918085474103
Mobile Platform 2 Pose: [-2.33919433259706;9.95163619837054;4.23471306672129]
Robot Arm 2 end: [9.30599162281634;4.25884874225377;0.248611132737155]
Robot Arm 1 togoal: 24.0344555466576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785393839545713;-9.94833893519495;-4.38562592554198]
Robot Arm 1 end: [-9.30329464760437;-4.37907081202582;0.247113340681576]
Robot Arm 1 togoal: 24.1038928161049
Mobile Platform 2 Pose: [-2.33919420319653;9.95009875735837;4.0815392754452]
Robot Arm 2 end: [9.30448475399131;4.10560521871282;0.248590003406799]
Robot Arm 1 togoal: 23.9426451630285

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.785393709935777;-9.94677087325184;-4.24147269120878]
Robot Arm 1 end: [-9.30188376583759;-4.23498769296685;0.246901271378309]
Robot Arm 1 togoal: 24.0170963919267
Mobile Platform 2 Pose: [-2.33919407357555;9.94855484019966;3.92938781976183]
Robot Arm 2 end: [9.30309304713044;3.95337944588015;0.248387064011601]
Robot Arm 1 togoal: 23.8521643217493

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785393580125884;-9.94519788304917;-4.09828460127338]
Robot Arm 1 end: [-9.30046703773339;-4.09186977935385;0.246690610280352]
Robot Arm 1 togoal: 23.9314163010388
Mobile Platform 2 Pose: [-2.33919394373479;9.94700450146479;3.77826408826941]
Robot Arm 2 end: [9.30173022172881;3.80218002057367;0.248131432661844]
Robot Arm 1 togoal: 23.7629366296011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785393450120758;-9.94361999644825;-3.95606523152243]
Robot Arm 1 end: [-9.29904447529772;-3.94972064447447;0.246481376104443]
Robot Arm 1 togoal: 23.8468492979902
Mobile Platform 2 Pose: [-2.33919381367797;9.945447784998;3.62817297404917]
Robot Arm 2 end: [9.30037069401279;3.65201278685228;0.247861444553698]
Robot Arm 1 togoal: 23.6749370398329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785393319925179;-9.94203724614072;-3.81481800354123]
Robot Arm 1 end: [-9.29761606378089;-3.80854370714073;0.246273628713517]
Robot Arm 1 togoal: 23.7633918534111
Mobile Platform 2 Pose: [-2.33919368340981;9.94388473227113;3.479119099135]
Robot Arm 2 end: [9.29900687962944;3.50288265055364;0.247588494412046]
Robot Arm 1 togoal: 23.5881555717519

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785393189543977;-9.94044966562789;-3.67454618194975]
Robot Arm 1 end: [-9.29618181286199;-3.66834222945029;0.246067403520059]
Robot Arm 1 togoal: 23.6810402005902
Mobile Platform 2 Pose: [-2.33919355293538;9.94231538487602;3.33110687759665]
Robot Arm 2 end: [9.29763650988089;3.35479411314416;0.247316034215948]
Robot Arm 1 togoal: 23.5025864856754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785393058982036;-9.9388572891924;-3.53525287213257]
Robot Arm 1 end: [-9.29474173132081;-3.52911931421131;0.245862730618121]
Robot Arm 1 togoal: 23.5997903158546
Mobile Platform 2 Pose: [-2.33919342225989;9.94073978523953;3.18414053147223]
Robot Arm 2 end: [9.29625895068923;3.20775142399261;0.247045056480497]
Robot Arm 1 togoal: 23.4182251327497

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.786643254170027;-9.93726015648492;-3.39694102903087]
Robot Arm 1 end: [-9.29329596485651;-3.39086386097597;0.245659638995813]
Robot Arm 1 togoal: 23.5196300389089
Mobile Platform 2 Pose: [-2.33919329138864;9.93915797685534;3.03822409325702]
Robot Arm 2 end: [9.29487398900569;3.06175862747021;0.246775935175729]
Robot Arm 1 togoal: 23.3350669092951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.787894979673491;-9.93565828359982;-3.25961570802449]
Robot Arm 1 end: [-9.29184450531877;-3.25359499583335;0.245457977389533]
Robot Arm 1 togoal: 23.4405642462468
Mobile Platform 2 Pose: [-2.33909182139326;9.93757000452695;2.89336140426911]
Robot Arm 2 end: [9.36985053062211;2.9140278643605;0.15862298375747]
Robot Arm 1 togoal: 23.319933456048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.789147931727298;-9.93405172172334;-3.12327953210596]
Robot Arm 1 end: [-9.29038724990734;-3.11731534149432;0.245257961646388]
Robot Arm 1 togoal: 23.3625880366027
Mobile Platform 2 Pose: [-2.33665521045716;9.93597217593275;2.74982732523273]
Robot Arm 2 end: [9.33447023680289;2.77166258335569;0.18951442622396]
Robot Arm 1 togoal: 23.2101262020282

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.790401737099624;-9.93244050885592;-2.98793493063772]
Robot Arm 1 end: [-9.28892421230394;-2.9820273289227;0.245059604916087]
Robot Arm 1 togoal: 23.2856963548164
Mobile Platform 2 Pose: [-2.33419196046135;9.93437842872535;2.60722454222205]
Robot Arm 2 end: [9.30696059103023;2.62989436722681;0.222280342922443]
Robot Arm 1 togoal: 23.1075665473148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.79165598451537;-9.93082467386767;-2.85358416219153]
Robot Arm 1 end: [-9.28745537102632;-2.8477332185272;0.244862973503791]
Robot Arm 1 togoal: 23.2098838698985
Mobile Platform 2 Pose: [-2.33401928621832;9.93277557734875;2.46560034026236]
Robot Arm 2 end: [9.37763321176877;2.48559271564781;0.14411101362638]
Robot Arm 1 togoal: 23.092823942335

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.792910169905877;-9.92920426014314;-2.72022937701191]
Robot Arm 1 end: [-9.28598075364581;-2.7144351637655;0.244668095687917]
Robot Arm 1 togoal: 23.1351450890968
Mobile Platform 2 Pose: [-2.33167843189409;9.93116348243884;2.32529297564249]
Robot Arm 2 end: [9.34182400930134;2.34645896341907;0.173764325473448]
Robot Arm 1 togoal: 22.9860011508068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.794163714361129;-9.92757930323444;-2.58787243204526]
Robot Arm 1 end: [-9.28450035982794;-2.58213502535804;0.244475009791677]
Robot Arm 1 togoal: 23.0614741934503
Mobile Platform 2 Pose: [-2.32949336213883;9.92955682276137;2.1859269514951]
Robot Arm 2 end: [9.31370428344596;2.2079413872951;0.205498455634217]
Robot Arm 1 togoal: 22.8863179733495

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.795415954914986;-9.92594985855675;-2.45651508821614]
Robot Arm 1 end: [-9.28301422678145;-2.45083456911255;0.244283738150899]
Robot Arm 1 togoal: 22.9888652079857
Mobile Platform 2 Pose: [-2.32752957933526;9.92794076821075;2.04754628301348]
Robot Arm 2 end: [9.29263274833363;2.07014640384061;0.233291766225676]
Robot Arm 1 togoal: 22.7936900274517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.796666073497718;-9.9243159462797;-2.32615895632521]
Robot Arm 1 end: [-9.28152235757503;-2.32053541242825;0.244094300706302]
Robot Arm 1 togoal: 22.9173119156557
Mobile Platform 2 Pose: [-2.32585135359962;9.92631303838019;1.91018576280176]
Robot Arm 2 end: [9.285687475604;1.93288168144661;0.241421161559842]
Robot Arm 1 togoal: 22.7149450502055

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.797913110731245;-9.92267762146823;-2.19680537344788]
Robot Arm 1 end: [-9.28002477518105;-2.19123890039774;0.243906737279735]
Robot Arm 1 togoal: 22.8468078396053
Mobile Platform 2 Pose: [-2.32451856426121;9.92467800800085;1.77389048446976]
Robot Arm 2 end: [9.28264470564124;1.7965503956555;0.243644251462849]
Robot Arm 1 togoal: 22.64090602846

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.799155913093311;-9.92103492369333;-2.06845555414246]
Robot Arm 1 end: [-9.27852149766547;-2.06294625784615;0.243721076539967]
Robot Arm 1 togoal: 22.7773463117183
Mobile Platform 2 Pose: [-2.3235831429964;9.92303680572066;1.63867268231544]
Robot Arm 2 end: [9.28072101952979;1.66126219920606;0.244123525837964]
Robot Arm 1 togoal: 22.5690419079286

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.800393083998513;-9.91938789800271;-1.94111055398459]
Robot Arm 1 end: [-9.27701258326037;-1.93565855353452;0.243537288385135]
Robot Arm 1 togoal: 22.7089204988107
Mobile Platform 2 Pose: [-2.32308538039529;9.92138982089646;1.50453636350561]
Robot Arm 2 end: [9.27911867288089;1.52704915987267;0.244091614922553]
Robot Arm 1 togoal: 22.4986171610841

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.801622923038127;-9.91773657906731;-1.81477126452941]
Robot Arm 1 end: [-9.27549801900931;-1.80937669511352;0.243355440375221]
Robot Arm 1 togoal: 22.6415233018598
Mobile Platform 2 Pose: [-2.32437637657885;9.91973718082531;1.37148317049347]
Robot Arm 2 end: [9.27760580436461;1.39393605552665;0.243911406496669]
Robot Arm 1 togoal: 22.4294186206767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.801622789728476;-9.91608102217392;-1.68943828759175]
Robot Arm 1 end: [-9.27397771359684;-1.68411498339273;0.243175575384646]
Robot Arm 1 togoal: 22.5751543762056
Mobile Platform 2 Pose: [-2.32565351997804;9.91807898557811;1.23951131697589]
Robot Arm 2 end: [9.27611367610025;1.26190317884365;0.243689629439894]
Robot Arm 1 togoal: 22.3613627367906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.801622656315304;-9.91442126846259;-1.56510981501911]
Robot Arm 1 end: [-9.27245179086623;-1.55985778712928;0.242997727457102]
Robot Arm 1 togoal: 22.5097978644532
Mobile Platform 2 Pose: [-2.32691424825429;9.91641529920305;1.10862205676406]
Robot Arm 2 end: [9.27462251073569;1.13095243363454;0.243457461461134]
Robot Arm 1 togoal: 22.2944228440426

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.801622522804008;-9.91275736193912;-1.44178618466277]
Robot Arm 1 end: [-9.27092030844575;-1.43660544189722;0.242821878549711]
Robot Arm 1 togoal: 22.4454461501271
Mobile Platform 2 Pose: [-2.32815533449619;9.9147461914859;0.978816397502875]
Robot Arm 2 end: [9.2731265715454;1.0010849915339;0.243224032087182]
Robot Arm 1 togoal: 22.228584981009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.801622389199989;-9.91108934700622;-1.31946755612587]
Robot Arm 1 end: [-9.269383311392;-1.31435810493602;0.242648031972717]
Robot Arm 1 togoal: 22.3820914188608
Mobile Platform 2 Pose: [-2.32937270565286;9.91307168759742;0.850095103701028]
Robot Arm 2 end: [9.27162414274469;0.872301633478956;0.242992077942699]
Robot Arm 1 togoal: 22.1638386602459

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.801622255508654;-9.90941726843215;-1.19815391088924]
Robot Arm 1 end: [-9.26784085359265;-1.19311575533063;0.242476161412752]
Robot Arm 1 togoal: 22.319725685533
Mobile Platform 2 Pose: [-2.33056114244526;9.91139188996708;0.722458716681988]
Robot Arm 2 end: [9.27011483601695;0.744602862245134;0.242762360108266]
Robot Arm 1 togoal: 22.1001743867482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.801622121735413;-9.90774117135058;-1.07784505283935]
Robot Arm 1 end: [-9.26629294699377;-1.07287819429762;0.242306307979964]
Robot Arm 1 togoal: 22.2583407508993
Mobile Platform 2 Pose: [-2.33171394438693;9.90970682335913;0.595907629115766]
Robot Arm 2 end: [9.2685984932525;0.617989005156234;0.242535151942865]
Robot Arm 1 togoal: 22.0375826890226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.801621987885678;-9.90606110122001;-0.958540608431057]
Robot Arm 1 end: [-9.26473962463502;-0.953645045872823;0.242138497903208]
Robot Arm 1 togoal: 22.1979282667888
Mobile Platform 2 Pose: [-2.33282245481563;9.90801654654477;0.470441998092062]
Robot Arm 2 end: [9.267075086122;0.492460129056845;0.242310574808792]
Robot Arm 1 togoal: 21.9760539810997

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.80162185396486;-9.90437710377846;-0.84024002752535]
Robot Arm 1 end: [-9.26318094469688;-0.835415757551472;0.241972698884796]
Robot Arm 1 togoal: 22.1384797487119
Mobile Platform 2 Pose: [-2.33387546432831;9.90632113034243;0.346061772090462]
Robot Arm 2 end: [9.26554468766153;0.368016060938354;0.242088609288665]
Robot Arm 1 togoal: 21.9155785572147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.801621719978363;-9.90268922505313;-0.72294258453604]
Robot Arm 1 end: [-9.26161693118397;-0.718189601191404;0.241808936592982]
Robot Arm 1 togoal: 22.0799865241685
Mobile Platform 2 Pose: [-2.33485855604317;9.90462064635369;0.222766708296286]
Robot Arm 2 end: [9.264007300183;0.244656403349609;0.241869346860735]
Robot Arm 1 togoal: 21.8561464424083

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.801621585931587;-9.90099751131781;-0.606647379097811]
Robot Arm 1 end: [-9.26004761231007;-0.601965673957667;0.241647238201882]
Robot Arm 1 togoal: 22.0224397755868
Mobile Platform 2 Pose: [-2.3357533942539;9.90291513194843;0.100556391811684]
Robot Arm 2 end: [9.26246296327226;0.122380541069246;0.241652763738066]
Robot Arm 1 togoal: 21.7977475116257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.801621451829922;-9.8993020090569;-0.491353337221455]
Robot Arm 1 end: [-9.25847302518264;-0.486742899444825;0.24148761485586]
Robot Arm 1 togoal: 21.9658305521207
Mobile Platform 2 Pose: [-2.33653731155169;9.90120467079734;-0.0205697446701486]
Robot Arm 2 end: [9.26091173560673;0.00118765955051766;0.241438878140902]
Robot Arm 1 togoal: 21.7403714814847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.801621317678748;-9.89760276494552;-0.377059212669028]
Robot Arm 1 end: [-9.25689321573246;-0.372520029003595;0.241330055144738]
Robot Arm 1 togoal: 21.9101497768037
Mobile Platform 2 Pose: [-2.33718369055062;9.8994893006647;-0.140612456158069]
Robot Arm 2 end: [9.25935360452237;-0.118923285977276;0.241227742739025]
Robot Arm 1 togoal: 21.6840078242673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.801621183483431;-9.89589982583957;-0.263763588491758]
Robot Arm 1 end: [-9.25530823236897;-0.259295643399813;0.241174560707738]
Robot Arm 1 togoal: 21.8553882458601
Mobile Platform 2 Pose: [-2.33766395464788;9.89776910668136;-0.259572654418136]
Robot Arm 2 end: [9.25778865965226;-0.237953519806711;0.241019327077733]
Robot Arm 1 togoal: 21.6286459600304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.801907461374469;-9.89419323874014;-0.151464877679071]
Robot Arm 1 end: [-9.25371811969284;-0.147064951491244;0.241021159751521]
Robot Arm 1 togoal: 21.801535138468
Mobile Platform 2 Pose: [-2.33795173759422;9.89604413945007;-0.377451436954035]
Robot Arm 2 end: [9.25621690936991;-0.355904424697001;0.240813671448395]
Robot Arm 1 togoal: 21.5742751016597

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.80198440218447;-9.89248303787699;-0.0401619001610501]
Robot Arm 1 end: [-9.25212287303332;-0.0358323206463331;0.240869849199485]
Robot Arm 1 togoal: 21.7485838318151
Mobile Platform 2 Pose: [-2.33802879724662;9.89431444891725;-0.494250006380978]
Robot Arm 2 end: [9.25463840129878;-0.472777398104005;0.240610762884482]
Robot Arm 1 togoal: 21.5208844397569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.801845745832496;-9.89076930888867;0.0701476907667909]
Robot Arm 1 end: [-9.25052256738199;0.0744045349055751;0.240720637220454]
Robot Arm 1 togoal: 21.696524599593
Mobile Platform 2 Pose: [-2.33802866328467;9.89258012147567;-0.6099697208799]
Robot Arm 2 end: [9.2530532538597;-0.588572297692077;0.240410629984034]
Robot Arm 1 togoal: 21.4684638511079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.8015004295954;-9.88905206086239;0.179466414552181]
Robot Arm 1 end: [-9.24891722528249;0.183648243441201;0.240573510240846]
Robot Arm 1 togoal: 21.6453474943219
Mobile Platform 2 Pose: [-2.33802852929406;9.89084120805808;-0.724612440599026]
Robot Arm 2 end: [9.25146153266513;-0.703290116661484;0.240213276428811]
Robot Arm 1 togoal: 21.4170026014362

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.800969714862805;-9.88733135353403;0.287796907292013]
Robot Arm 1 end: [-9.24730691640716;0.291901687327936;0.240428450336822]
Robot Arm 1 togoal: 21.5950424837898
Mobile Platform 2 Pose: [-2.3380283952805;9.88909777171112;-0.838180365370882]
Robot Arm 2 end: [9.24986326954336;-0.816933051500705;0.240018745036142]
Robot Arm 1 togoal: 21.3664893572225

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.800281230718288;-9.88560724778022;0.395141972570633]
Robot Arm 1 end: [-9.24569167785018;0.399167986949456;0.240285494761344]
Robot Arm 1 togoal: 21.5455993916534
Mobile Platform 2 Pose: [-2.33802826124971;9.88734987556455;-0.950675890125296]
Robot Arm 2 end: [9.24825854486364;-0.929503495358159;0.239827003198165]
Robot Arm 1 togoal: 21.3169127243691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.799463464588506;-9.88387976244737;0.501504367987114]
Robot Arm 1 end: [-9.24407155538369;0.505450225079523;0.240144603281433]
Robot Arm 1 togoal: 21.4970080709961
Mobile Platform 2 Pose: [-2.33802812720735;9.88559758281185;-1.06210160207276]
Robot Arm 2 end: [9.24664741935545;-1.04100403309363;0.239638054135356]
Robot Arm 1 togoal: 21.2682611902416

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.798542103188924;-9.88214895911084;0.606887147718143]
Robot Arm 1 end: [-9.24244660588904;0.61075174895035;0.240005787215727]
Robot Arm 1 togoal: 21.4492582686096
Mobile Platform 2 Pose: [-2.33802799315904;9.88384095666159;-1.17246027777953]
Robot Arm 2 end: [9.2450299498937;-1.15143743874753;0.239451896312147]
Robot Arm 1 togoal: 21.2205231489417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.797538696098939;-9.88041488456397;0.71129337197949]
Robot Arm 1 end: [-9.24081689114914;0.71507586473747;0.239869018097368]
Robot Arm 1 togoal: 21.4023397652944
Mobile Platform 2 Pose: [-2.33723453791895;9.88208006086039;-1.28175491377192]
Robot Arm 2 end: [9.24340589708457;-1.26081554931687;0.239268547117723]
Robot Arm 1 togoal: 21.173682979121

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.796470487645695;-9.87867757066086;0.814726137387167]
Robot Arm 1 end: [-9.23918242161564;0.818425866321975;0.23973433067345]
Robot Arm 1 togoal: 21.3562423085107
Mobile Platform 2 Pose: [-2.33613370883722;9.88031495813345;-1.38998698161055]
Robot Arm 2 end: [9.24177548927158;-1.36913439150045;0.239088020269384]
Robot Arm 1 togoal: 21.1277320944603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.795350941179796;-9.876937105884;0.917188762514315]
Robot Arm 1 end: [-9.23754329907388;0.920805226596425;0.239601704938771]
Robot Arm 1 togoal: 21.3109556685693
Mobile Platform 2 Pose: [-2.33474408683264;9.87854569553214;-1.49715929864399]
Robot Arm 2 end: [9.24013878182152;-1.47639656321862;0.238910307231269]
Robot Arm 1 togoal: 21.0826589848522

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.79419055499285;-9.87519348231472;1.01868469491891]
Robot Arm 1 end: [-9.23589952932103;1.02221751385153;0.239471123996273]
Robot Arm 1 togoal: 21.2664695079075
Mobile Platform 2 Pose: [-2.33469126581037;9.87677237171197;-1.6032746663553]
Robot Arm 2 end: [9.32763864510534;-1.58550400007476;0.141130117299335]
Robot Arm 1 togoal: 21.1249946608682

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.792997408006515;-9.87344680016787;1.11921733559143]
Robot Arm 1 end: [-9.23425121550511;1.12266622259674;0.239342573377965]
Robot Arm 1 togoal: 21.2227736306026
Mobile Platform 2 Pose: [-2.3327927255912;9.87498982393378;-1.70822880956543]
Robot Arm 2 end: [9.29460835553061;-1.68945431939613;0.165932060785491]
Robot Arm 1 togoal: 21.0519189201983

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.79177778551864;-9.87169706466883;1.2187903877466]
Robot Arm 1 end: [-9.23259834845466;1.22215512923394;0.239216077188218]
Robot Arm 1 togoal: 21.1798576443043
Mobile Platform 2 Pose: [-2.33068598204852;9.87321764717966;-1.81220376991116]
Robot Arm 2 end: [9.26828456451455;-1.79272692060577;0.192354134468167]
Robot Arm 1 togoal: 20.9855050283488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.790536595399583;-9.86994435074284;1.31740763653711]
Robot Arm 1 end: [-9.23094104301069;1.32068807627835;0.239091579337158]
Robot Arm 1 togoal: 21.1377112779027
Mobile Platform 2 Pose: [-2.32840817590149;9.87144240890387;-1.91516477226804]
Robot Arm 2 end: [9.24594334500012;-1.89515061145934;0.21987504026876]
Robot Arm 1 togoal: 20.9233803437857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.789277687125209;-9.86818867392107;1.41507287209395]
Robot Arm 1 end: [-9.22927928692962;1.41826889958215;0.238969114161919]
Robot Arm 1 togoal: 21.0963241766632
Mobile Platform 2 Pose: [-2.32824985855892;9.86965738167541;-2.01709169670633]
Robot Arm 2 end: [9.33127148544293;-1.99994849344553;0.129507023085407]
Robot Arm 1 togoal: 20.9674780089601

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.788004112449208;-9.86643009381588;1.51179007821194]
Robot Arm 1 end: [-9.22761316027046;1.51490161938637;0.238848649070776]
Robot Arm 1 togoal: 21.0556860362323
Mobile Platform 2 Pose: [-2.32576471701239;9.86786272119523;-2.11789137725869]
Robot Arm 2 end: [9.29800798227761;-2.09975600510789;0.153345331052536]
Robot Arm 1 togoal: 20.8973071604064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.788003979027409;-9.86466865797071;1.6075634947652]
Robot Arm 1 end: [-9.225942788826;1.61060483753063;0.238730170615748]
Robot Arm 1 togoal: 21.0157808141079
Mobile Platform 2 Pose: [-2.32569081591831;9.86608032992871;-2.21772285188874]
Robot Arm 2 end: [9.27133514214716;-2.19886667960073;0.178975397012666]
Robot Arm 1 togoal: 20.8337003261212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.788003845657567;-9.86290441409994;1.70239470846794]
Robot Arm 1 end: [-9.22426805719391;1.70536590912465;0.238613802077385]
Robot Arm 1 togoal: 20.9766048586748
Mobile Platform 2 Pose: [-2.32561677444144;9.86429512603073;-2.31655875364759]
Robot Arm 2 end: [9.24856686505666;-2.29714027174727;0.205776812937659]
Robot Arm 1 togoal: 20.7742914772919

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.788003712344332;-9.86113740751906;1.7962880750422]
Robot Arm 1 end: [-9.22258906156344;1.79918919157828;0.238499471655007]
Robot Arm 1 togoal: 20.9381478586448
Mobile Platform 2 Pose: [-2.3255426266077;9.86249972693265;-2.41438053098805]
Robot Arm 2 end: [9.2311771894186;-2.39457295177158;0.228336748349958]
Robot Arm 1 togoal: 20.7208029515969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.788003579092298;-9.85936768325814;1.88924806621196]
Robot Arm 1 end: [-9.22090590126653;1.89207915829657;0.238387101371133]
Robot Arm 1 togoal: 20.9003995150428
Mobile Platform 2 Pose: [-2.3254684233366;9.86069464457483;-2.51118019308204]
Robot Arm 2 end: [9.22508970318637;-2.49131758308103;0.234897013091463]
Robot Arm 1 togoal: 20.679420848434

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.788003445906003;-9.85759528604886;1.98127926632137]
Robot Arm 1 end: [-9.21921861106634;1.98404039549796;0.238276707405231]
Robot Arm 1 togoal: 20.8633494781239
Mobile Platform 2 Pose: [-2.32539419782143;9.85888443268608;-2.60696608994193]
Robot Arm 2 end: [9.22212770169055;-2.58714028806498;0.23671258358195]
Robot Arm 1 togoal: 20.6419494393552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.788003312789932;-9.85582026029576;2.07238636961851]
Robot Arm 1 end: [-9.21752723799125;2.0750775993062;0.238168279259307]
Robot Arm 1 togoal: 20.8269874303436
Mobile Platform 2 Pose: [-2.32531995694389;9.8570703627754;-2.70174579080151]
Robot Arm 2 end: [9.22005752295211;-2.68198285824001;0.237139410816094]
Robot Arm 1 togoal: 20.6061224902974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.788003179748515;-9.85404265007678;2.16257417669618]
Robot Arm 1 end: [-9.21583186862469;2.1651955719405;0.238061760367197]
Robot Arm 1 togoal: 20.7913031182729
Mobile Platform 2 Pose: [-2.3252457070865;9.85525282736705;-2.79552550110271]
Robot Arm 2 end: [9.21824173939105;-2.77583284116319;0.237162562354867]
Robot Arm 1 togoal: 20.5712820518991

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.788003046786123;-9.85226249911967;2.25184759072728]
Robot Arm 1 end: [-9.21413252796639;2.25439921841326;0.237957177104269]
Robot Arm 1 togoal: 20.7562862563455
Mobile Platform 2 Pose: [-2.32517145952691;9.85343196093157;-2.88831092491508]
Robot Arm 2 end: [9.21649608727532;-2.8686905795453;0.237069831993533]
Robot Arm 1 togoal: 20.5372289415795

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.788002913907076;-9.85047985078553;2.34021161463571]
Robot Arm 1 end: [-9.21242927169273;2.34269354341116;0.237854505880584]
Robot Arm 1 togoal: 20.7219266249302
Mobile Platform 2 Pose: [-2.32509721655926;9.85160784369254;-2.9801077292793]
Robot Arm 2 end: [9.21476713708634;-2.96056019354751;0.236945189205973]
Robot Arm 1 togoal: 20.5038968412896

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.788002781115632;-9.84869474806994;2.42767134731715]
Robot Arm 1 end: [-9.21072218136024;2.43008364746598;0.237753689019018]
Robot Arm 1 togoal: 20.6882140700548
Mobile Platform 2 Pose: [-2.32502297640567;9.84978057290478;-3.07092166916177]
Robot Arm 2 end: [9.2130394983053;-3.05144699221167;0.236813037199481]
Robot Arm 1 togoal: 20.4712581885167

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.78800264841599;-9.84690723359038;2.51423197994384]
Robot Arm 1 end: [-9.20901128785726;2.51657472337457;0.237654754602107]
Robot Arm 1 togoal: 20.6551384296196
Mobile Platform 2 Pose: [-2.32494874179641;9.84795018525492;-3.16075849095658]
Robot Arm 2 end: [9.21130870628306;-3.14135659197815;0.236680491941693]
Robot Arm 1 togoal: 20.4392966661108

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.788002515812293;-9.84511734954543;2.59989879310215]
Robot Arm 1 end: [-9.20729664256306;2.60217205341917;0.237557683433527]
Robot Arm 1 togoal: 20.62268961175
Mobile Platform 2 Pose: [-2.32487451645835;9.84611674646304;-3.24962411715716]
Robot Arm 2 end: [9.20957354240069;-3.23029487748475;0.236549607964064]
Robot Arm 1 togoal: 20.4079993124341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.788002383308625;-9.84332513773396;2.68467715305224]
Robot Arm 1 end: [-9.20557831831071;2.68688100551348;0.23746242158705]
Robot Arm 1 togoal: 20.5908575995145
Mobile Platform 2 Pose: [-2.3248002896374;9.84428031385448;-3.33752457853233]
Robot Arm 2 end: [9.20783367718105;-3.31826786706409;0.236420990406847]
Robot Arm 1 togoal: 20.3773541486241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.788002250909006;-9.84153063955438;2.76857250804981]
Robot Arm 1 end: [-9.203856355755;2.770707029427;0.237368980848896]
Robot Arm 1 togoal: 20.5596323989554
Mobile Platform 2 Pose: [-2.32472604625621;9.8424409344965;-3.42446591813558]
Robot Arm 2 end: [9.20608911203066;-3.40528160115666;0.236294720296718]
Robot Arm 1 togoal: 20.3473496405552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.788002118617398;-9.83973389595886;2.8515903853522]
Robot Arm 1 end: [-9.20213079732115;2.85365565400366;0.237277358150391]
Robot Arm 1 togoal: 20.5290040793552
Mobile Platform 2 Pose: [-2.32465179519794;9.84059867964204;-3.5104543429796]
Robot Arm 2 end: [9.20433986341236;-3.49134228361696;0.236170883537283]
Robot Arm 1 togoal: 20.317974326123

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.788001986437704;-9.83793494746265;2.9337363877286]
Robot Arm 1 end: [-9.2004017390786;2.93573248349842;0.237187469221329]
Robot Arm 1 togoal: 20.4989628298823
Mobile Platform 2 Pose: [-2.32457753118789;9.83875359677944;-3.595496157255]
Robot Arm 2 end: [9.20258600049582;-3.57645621787134;0.236049453865893]
Robot Arm 1 togoal: 20.2892168831962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.788001854373759;-9.83613383415159;3.01501618948603]
Robot Arm 1 end: [-9.19866919013813;3.0169431938301;0.237099356755489]
Robot Arm 1 togoal: 20.4694988187859
Mobile Platform 2 Pose: [-2.32450324446027;9.83690573937937;-3.67959769497917]
Robot Arm 2 end: [9.20082760257312;-3.66062973727508;0.235930389768939]
Robot Arm 1 togoal: 20.2610661085441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.788113328873057;-9.83433059566451;3.09543553448979]
Robot Arm 1 end: [-9.19693323369704;3.09729477180163;0.237012970831434]
Robot Arm 1 togoal: 20.4406019416624
Mobile Platform 2 Pose: [-2.32442891967939;9.83505517000006;-3.76276544384742]
Robot Arm 2 end: [9.19906473804515;-3.74386932842112;0.235813682548458]
Robot Arm 1 togoal: 20.2335108586247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.788275942478698;-9.83252525425291;3.17499998427693]
Robot Arm 1 end: [-9.1951938636928;3.17679210450069;0.236928342231656]
Robot Arm 1 togoal: 20.4122628209671
Mobile Platform 2 Pose: [-2.32435457301626;9.8332019322952;-3.84500587058173]
Robot Arm 2 end: [9.19729747562422;-3.82618145717029;0.235699297197648]
Robot Arm 1 togoal: 20.2065401210603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.78822085531783;-9.83071786650914;3.25371534093558]
Robot Arm 1 end: [-9.19345118042469;3.25543800362878;0.236845400453036]
Robot Arm 1 togoal: 20.3844729579594
Mobile Platform 2 Pose: [-2.32428019435057;9.83134608081959;-3.92632567115891]
Robot Arm 2 end: [9.19552588842419;-3.90757281879631;0.235587210119103]
Robot Arm 1 togoal: 20.1801429413129

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.7878978359601;-9.82890848682531;3.3315879681454]
Robot Arm 1 end: [-9.19170527165928;3.33323828227484;0.236764095946616]
Robot Arm 1 togoal: 20.3572229169998
Mobile Platform 2 Pose: [-2.32420576940449;9.82948763969928;-4.00673140251389]
Robot Arm 2 end: [9.1937500113816;-3.9880499693216;0.235477402463875]
Robot Arm 1 togoal: 20.1543084976776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.78750166851289;-9.82709715154065;3.40862438336081]
Robot Arm 1 end: [-9.18995621986884;3.41020163336921;0.236684380424964]
Robot Arm 1 togoal: 20.3305024247808
Mobile Platform 2 Pose: [-2.32413130468677;9.82762670268179;-4.08622983790035]
Robot Arm 2 end: [9.19196997373151;-4.06761968155717;0.235369818322118]
Robot Arm 1 togoal: 20.1290261234691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.787408875109203;-9.82528386219146;3.48483080285598]
Robot Arm 1 end: [-9.18820401920144;3.48633846344531;0.236606274079993]
Robot Arm 1 togoal: 20.304300710403
Mobile Platform 2 Pose: [-2.32431212184366;9.82576330406373;-4.16482777094973]
Robot Arm 2 end: [9.29446844801951;-4.14937405905975;0.125049975663937]
Robot Arm 1 togoal: 20.2102699898558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.787408743988814;-9.82346867491569;3.56021265812011]
Robot Arm 1 end: [-9.18644869209932;3.5616518534903;0.236529804573112]
Robot Arm 1 togoal: 20.2786095172515
Mobile Platform 2 Pose: [-2.32607557941935;9.82389079457356;-4.2426362077818]
Robot Arm 2 end: [9.26310396914478;-4.22625996169119;0.146711082013703]
Robot Arm 1 togoal: 20.1566842160647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.787408613012601;-9.82165162657076;3.63477588713852]
Robot Arm 1 end: [-9.18469031051072;3.63614670968166;0.23645492860469]
Robot Arm 1 togoal: 20.2534201321985
Mobile Platform 2 Pose: [-2.32755006396515;9.82203099303069;-4.31957792598751]
Robot Arm 2 end: [9.23591220044946;-4.3024794500302;0.169693982450329]
Robot Arm 1 togoal: 20.1074137211575

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.787408482183842;-9.81983275296192;3.70852666457339]
Robot Arm 1 end: [-9.18292890686366;3.7098292079543;0.23638165162279]
Robot Arm 1 togoal: 20.2287234975388
Mobile Platform 2 Pose: [-2.32871221774335;9.82017635912259;-4.39563425462195]
Robot Arm 2 end: [9.21353088382218;-4.37802270059533;0.19406789384968]
Robot Arm 1 togoal: 20.0630791902176

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.787408351505754;-9.81801208939513;3.78147120507031]
Robot Arm 1 end: [-9.18116456741889;3.78270556421143;0.236309896619663]
Robot Arm 1 togoal: 20.2045107073994
Mobile Platform 2 Pose: [-2.32954594458604;9.81831490002184;-4.47078567129378]
Robot Arm 2 end: [9.19356242476096;-4.45275359782475;0.219093208583]
Robot Arm 1 togoal: 20.0214820253941

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.787408220981486;-9.81618967069259;3.85361575965111]
Robot Arm 1 end: [-9.17939732301293;3.85478203064808;0.236239674522586]
Robot Arm 1 togoal: 20.1807728954756
Mobile Platform 2 Pose: [-2.3300417035335;9.81644397355229;-4.54502408325048]
Robot Arm 2 end: [9.18416364890985;-4.52685767412758;0.230320530380962]
Robot Arm 1 togoal: 19.9913300037968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.78740809061413;-9.81436553116606;3.92496661341861]
Robot Arm 1 end: [-9.17762723406688;3.92606489354236;0.236170948820924]
Robot Arm 1 togoal: 20.1575013257357
Mobile Platform 2 Pose: [-2.33019567047033;9.81456786699093;-4.6183797453367]
Robot Arm 2 end: [9.18019492560496;-4.60022510411289;0.233544682982124]
Robot Arm 1 togoal: 19.9673480338219

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.787407960406712;-9.81253970462801;3.99553008231256]
Robot Arm 1 end: [-9.17585434615125;3.99656046996626;0.236103705829969]
Robot Arm 1 togoal: 20.1346873475192
Mobile Platform 2 Pose: [-2.33000908800082;9.8126887600738;-4.69087629967815]
Robot Arm 2 end: [9.17776077816128;-4.67277710075433;0.234420520407284]
Robot Arm 1 togoal: 19.9454530918688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.787407830362201;-9.81071222438446;4.0653125104402]
Robot Arm 1 end: [-9.17407872127757;4.06627510520231;0.236037901323609]
Robot Arm 1 togoal: 20.1123224300906
Mobile Platform 2 Pose: [-2.3294875269768;9.81080735843361;-4.76252568346725]
Robot Arm 2 end: [9.17576601055383;-4.74449693290015;0.234612425924077]
Robot Arm 1 togoal: 19.924485126862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.787407700483497;-9.80888312324742;4.13432026706896]
Robot Arm 1 end: [-9.17230039308667;4.13521516959598;0.235973537882306]
Robot Arm 1 togoal: 20.0903981164643
Mobile Platform 2 Pose: [-2.32864049920138;9.80892381044069;-4.833336027565]
Robot Arm 2 end: [9.17389572372982;-4.81538445392125;0.234606591378243]
Robot Arm 1 togoal: 19.9041031846916

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.787407570773446;-9.80705243351866;4.20255974424332]
Robot Arm 1 end: [-9.17051941911104;4.20338705631902;0.23591057648729]
Robot Arm 1 togoal: 20.0689060798985
Mobile Platform 2 Pose: [-2.32748145146834;9.80703827796841;-4.9033142668504]
Robot Arm 2 end: [9.17205897870554;-4.88544409683983;0.234544640397447]
Robot Arm 1 togoal: 19.8842021454662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.787407441234831;-9.80522018700509;4.27003735380328]
Robot Arm 1 end: [-9.16873586943725;4.27079717826342;0.235848964499234]
Robot Arm 1 togoal: 20.0478381125969
Mobile Platform 2 Pose: [-2.32602814426825;9.80515077898222;-4.97246726807757]
Robot Arm 2 end: [9.17022940324119;-4.95468184824348;0.234467822403367]
Robot Arm 1 togoal: 19.8647450441562

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.78740731187037;-9.8033864150144;4.33675952465463]
Robot Arm 1 end: [-9.16694974746033;4.33745196539349;0.235788740335379]
Robot Arm 1 togoal: 20.0271860441717
Mobile Platform 2 Pose: [-2.32589581616814;9.80326136646004;-5.04080151564297]
Robot Arm 2 end: [9.27400445500354;-5.02608604455372;0.123361569281376]
Robot Arm 1 togoal: 19.9544596757392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.787407182682728;-9.80155114834813;4.40273270092204]
Robot Arm 1 end: [-9.1651611129039;4.4033578627019;0.235729875539116]
Robot Arm 1 togoal: 20.0069418699954
Mobile Platform 2 Pose: [-2.32325604556989;9.80136274054048;-5.10853765813435]
Robot Arm 2 end: [9.24326902293014;-5.09298461862592;0.144261696653401]
Robot Arm 1 togoal: 19.9067227588522

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.78740705367451;-9.79971441729248;4.46796333890756]
Robot Arm 1 end: [-9.16337003428864;4.46852132764765;0.235672303223667]
Robot Arm 1 togoal: 19.987097704816
Mobile Platform 2 Pose: [-2.32124202632425;9.79947765854562;-5.17545916439776]
Robot Arm 2 end: [9.21627631316859;-5.15924838100356;0.166417579671694]
Robot Arm 1 togoal: 19.8628559153593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.787406924848259;-9.79787625166039;4.53245790439333]
Robot Arm 1 end: [-9.16157654875057;4.53294882693256;0.235616017670127]
Robot Arm 1 togoal: 19.9676457386592
Mobile Platform 2 Pose: [-2.31995001701614;9.79759961152929;-5.24155265639105]
Robot Arm 2 end: [9.19415381801105;-5.22489063534777;0.189991909045965]
Robot Arm 1 togoal: 19.8239449346502

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.787406796206459;-9.7960366807612;4.59622287071198]
Robot Arm 1 end: [-9.15978068910119;4.59664683482327;0.235561014929086]
Robot Arm 1 togoal: 19.9485782667944
Mobile Platform 2 Pose: [-2.31945138836102;9.79571567700925;-5.30680187382231]
Robot Arm 2 end: [9.17441838682957;-5.28976905549381;0.214226183068894]
Robot Arm 1 togoal: 19.7876793442887

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.787406667751539;-9.79419573340776;4.65926471644727]
Robot Arm 1 end: [-9.15798251826731;4.65962183076438;0.23550725315487]
Robot Arm 1 togoal: 19.929887725797
Mobile Platform 2 Pose: [-2.31978392818045;9.79382282255514;-5.37120250368987]
Robot Arm 2 end: [9.16294263796129;-5.35399942877724;0.228378821238155]
Robot Arm 1 togoal: 19.7604291796859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.787406539485868;-9.79235343791643;4.72158992286876]
Robot Arm 1 end: [-9.15618204760623;4.7218802969238;0.235454754905831]
Robot Arm 1 togoal: 19.9115666088386
Mobile Platform 2 Pose: [-2.32094619765938;9.79192429468456;-5.43478836143214]
Robot Arm 2 end: [9.15837295244987;-5.41757422393642;0.23247718814387]
Robot Arm 1 togoal: 19.740875644585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.787406411411765;-9.79050982211229;4.78320497225455]
Robot Arm 1 end: [-9.15437935493749;4.78342871651024;0.235403447971772]
Robot Arm 1 togoal: 19.8936075894538
Mobile Platform 2 Pose: [-2.32289674469956;9.79002301583049;-5.49759906238652]
Robot Arm 2 end: [9.15574153877354;-5.48041193818558;0.233621593042822]
Robot Arm 1 togoal: 19.7237535299823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.787406283531486;-9.7886649133449;4.84411634500306]
Robot Arm 1 end: [-9.15257448313592;4.84427357084168;0.235353306348254]
Robot Arm 1 togoal: 19.8760034153068
Mobile Platform 2 Pose: [-2.32326671076468;9.78811977440672;-5.55965291610264]
Robot Arm 2 end: [9.25513322862867;-5.54527538611407;0.125011127571242]
Robot Arm 1 togoal: 19.8129632304142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.787406155847229;-9.78681873848904;4.90433051792694]
Robot Arm 1 end: [-9.15076744981529;4.90442133738365;0.235304355844763]
Robot Arm 1 togoal: 19.8587469169939
Mobile Platform 2 Pose: [-2.32652579239545;9.78621147949677;-5.62123869856352]
Robot Arm 2 end: [9.22469013854627;-5.6059967545599;0.145620003081388]
Robot Arm 1 togoal: 19.768349591708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.787406028361146;-9.78497132391801;4.96385396262482]
Robot Arm 1 end: [-9.14895832340646;4.96387848872691;0.235256526103726]
Robot Arm 1 togoal: 19.8418310906159
Mobile Platform 2 Pose: [-2.32854238906914;9.78431884098942;-5.68206507422614]
Robot Arm 2 end: [9.19942024190864;-5.66621778737803;0.167751791055722]
Robot Arm 1 togoal: 19.7289301941808

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.787405901075327;-9.78312269555306;5.02269314279119]
Robot Arm 1 end: [-9.14714712984214;5.02265148920787;0.235209827898513]
Robot Arm 1 togoal: 19.8252489965796
Mobile Platform 2 Pose: [-2.32984036619707;9.78242920403914;-5.74210957794385]
Robot Arm 2 end: [9.17805781886712;-5.72582840982928;0.191089394862315]
Robot Arm 1 togoal: 19.6935295643948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.787405773991811;-9.78127287882638;5.08085451293942]
Robot Arm 1 end: [-9.14533393286664;5.08074679418709;0.235164204464059]
Robot Arm 1 togoal: 19.8089938461537
Mobile Platform 2 Pose: [-2.33113837363163;9.78053250580728;-5.80135734735444]
Robot Arm 2 end: [9.15876035204182;-5.78471447008919;0.21498258218683]
Robot Arm 1 togoal: 19.6604275594728

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.787405647112582;-9.77942189871295;5.13834451604174]
Robot Arm 1 end: [-9.14351873431615;5.13817084731729;0.235119690003838]
Robot Arm 1 togoal: 19.79305889479
Mobile Platform 2 Pose: [-2.33243643529352;9.77862686212806;-5.85980430169822]
Robot Arm 2 end: [9.14778029950023;-5.84300087177359;0.22845876222886]
Robot Arm 1 togoal: 19.6364150241673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.787405520439576;-9.77756977971684;5.19516958247912]
Robot Arm 1 end: [-9.14170159593268;5.19493007973122;0.235076233448111]
Robot Arm 1 togoal: 19.7774375706204
Mobile Platform 2 Pose: [-2.33373457288299;9.77671591159139;-5.91749043377292]
Robot Arm 2 end: [9.14333660380337;-5.90067954289249;0.232360446068745]
Robot Arm 1 togoal: 19.6196930770217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.787405393974677;-9.77571654588718;5.25133612763322]
Robot Arm 1 end: [-9.13988255460504;5.25103090756549;0.235033813758142]
Robot Arm 1 togoal: 19.7621233848271
Mobile Platform 2 Pose: [-2.33503276370502;9.77480250847074;-5.97445774704817]
Robot Arm 2 end: [9.14072743359487;-5.95768194845971;0.233453712520224]
Robot Arm 1 togoal: 19.6052431089079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.787405267719714;-9.77386222082487;5.30685055047541]
Robot Arm 1 end: [-9.13806165912762;5.30647973045608;0.2349923995256]
Robot Arm 1 togoal: 19.7471099692504
Mobile Platform 2 Pose: [-2.33633098392357;9.77288747794098;-6.0307239524912]
Robot Arm 2 end: [9.13864283366159;-6.01399526593931;0.233729526844868]
Robot Arm 1 togoal: 19.5916550736654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.787405141676466;-9.77200682767906;5.36171923187043]
Robot Arm 1 end: [-9.13623892030053;5.36128292982187;0.234952013363556]
Robot Arm 1 togoal: 19.7323910232875
Mobile Platform 2 Pose: [-2.33745561530652;9.77097102337705;-6.086299211995]
Robot Arm 2 end: [9.13670781736038;-6.06962291534859;0.233768646126898]
Robot Arm 1 togoal: 19.5785192155323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.787405015846669;-9.77015038914324;5.41594853346973]
Robot Arm 1 end: [-9.13441439491945;5.41544686811647;0.234912599651135]
Robot Arm 1 togoal: 19.7179604030665
Mobile Platform 2 Pose: [-2.33899254410802;9.76905327269449;-6.14119133336016]
Robot Arm 2 end: [9.13481446523335;-6.12456373617006;0.233740055637123]
Robot Arm 1 togoal: 19.5657141198136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.787565469524306;-9.76829293963927;5.46954478444496]
Robot Arm 1 end: [-9.13258813437353;5.46897965679629;0.234874133647736]
Robot Arm 1 togoal: 19.7038116569916
Mobile Platform 2 Pose: [-2.34088548319927;9.76713421425504;-6.19540844725728]
Robot Arm 2 end: [9.13293165605032;-6.17882578916179;0.233692631694238]
Robot Arm 1 togoal: 19.5531993894902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.787810834753873;-9.76643447728705;5.52251399083509]
Robot Arm 1 end: [-9.21804388760292;5.52191502956475;0.140444383219717]
Robot Arm 1 togoal: 19.7811119424388
Mobile Platform 2 Pose: [-2.34121520782058;9.76521398657847;-6.24895836975682]
Robot Arm 2 end: [9.23284938539514;-6.23515365130144;0.126361006842479]
Robot Arm 1 togoal: 19.6474319296909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.790498847261745;-9.76457304825195;5.57403687549545]
Robot Arm 1 end: [-9.18700325182333;5.57350801138093;0.163053433023316]
Robot Arm 1 togoal: 19.737754279288
Mobile Platform 2 Pose: [-2.34360629996983;9.7632842349692;-6.30221541924755]
Robot Arm 2 end: [9.20306471539614;-6.2875824046839;0.14656898305575]
Robot Arm 1 togoal: 19.6068559306698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.792572287845326;-9.76272350926503;5.62516875676623]
Robot Arm 1 end: [-9.16192571263432;5.62458361057588;0.187105735750029]
Robot Arm 1 togoal: 19.7003284111092
Mobile Platform 2 Pose: [-2.34613498586593;9.76136905053582;-6.35477363438957]
Robot Arm 2 end: [9.17702980540031;-6.33949987327104;0.16799250551187]
Robot Arm 1 togoal: 19.5700884672281

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.793924066385635;-9.7608741588129;5.67591052705424]
Robot Arm 1 end: [-9.14069862448625;5.6752089990803;0.212231714200251]
Robot Arm 1 togoal: 19.6668003361238
Mobile Platform 2 Pose: [-2.3487244874917;9.75946150269106;-6.40662045921304]
Robot Arm 2 end: [9.15564136950499;-6.39091081968953;0.190760408872275]
Robot Arm 1 togoal: 19.5380054856766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.794469256827339;-9.7590178583142;5.72626212104165]
Robot Arm 1 end: [-9.12778764978445;5.72545429347307;0.228198897141993]
Robot Arm 1 togoal: 19.6421825687469
Mobile Platform 2 Pose: [-2.35130136312122;9.75754902164131;-6.45774010083565]
Robot Arm 2 end: [9.13651104878885;-6.44167620817078;0.214150590335579]
Robot Arm 1 togoal: 19.5083463857769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.794157756997938;-9.75715640338003;5.77615111282766]
Robot Arm 1 end: [-9.12281740045334;5.77526114665689;0.232874728680814]
Robot Arm 1 togoal: 19.6262536013534
Mobile Platform 2 Pose: [-2.35379804365702;9.75562842747627;-6.50812808612273]
Robot Arm 2 end: [9.12525178852391;-6.49189386041445;0.227922142973323]
Robot Arm 1 togoal: 19.4872808520165

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.79298393213465;-9.75529286017838;5.82549078414392]
Robot Arm 1 end: [-9.12007676517758;5.82451904937107;0.234218173619064]
Robot Arm 1 togoal: 19.612949399416
Mobile Platform 2 Pose: [-2.35406470965528;9.7537028253554;-6.55782948084738]
Robot Arm 2 end: [9.2215270247692;-6.54423730057006;0.125679125252215]
Robot Arm 1 togoal: 19.579487157283

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.792715970821541;-9.75342810108432;5.87426110970149]
Robot Arm 1 end: [-9.20977385630268;5.8733108029706;0.136002864309852]
Robot Arm 1 togoal: 19.696781270173
Mobile Platform 2 Pose: [-2.35623031328363;9.75176738888351;-6.60730466793513]
Robot Arm 2 end: [9.19185743138926;-6.59289627858345;0.14567574147307]
Robot Arm 1 togoal: 19.540400821154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.789997083263706;-9.75156093824663;5.92158118877031]
Robot Arm 1 end: [-9.17869982781731;5.92063641744166;0.158268713914373]
Robot Arm 1 togoal: 19.6550444131347
Mobile Platform 2 Pose: [-2.35815938384063;9.74984741007202;-6.65611151025549]
Robot Arm 2 end: [9.16624284887771;-6.64109512369589;0.166955487568801]
Robot Arm 1 togoal: 19.5053978029637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.786666292774602;-9.74970564725631;5.96856440244931]
Robot Arm 1 end: [-9.15309881567522;5.9674998124298;0.181938490304173]
Robot Arm 1 togoal: 19.6187496475316
Mobile Platform 2 Pose: [-2.3598160767998;9.74793442014762;-6.70423559312684]
Robot Arm 2 end: [9.14503259598332;-6.68880932959025;0.189545082098276]
Robot Arm 1 togoal: 19.4748306528061

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.782866240081388;-9.74785172689549;6.01521451650662]
Robot Arm 1 end: [-9.1316003964469;6.01396256613954;0.20676156120548]
Robot Arm 1 togoal: 19.5865732282977
Mobile Platform 2 Pose: [-2.36117249682403;9.7460162310892;-6.75166119416615]
Robot Arm 2 end: [9.12599592955254;-6.73590647041952;0.212736772226765]
Robot Arm 1 togoal: 19.4465827214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.780410424277211;-9.74599104735537;6.06153056878103]
Robot Arm 1 end: [-9.11591000035773;6.06011253267453;0.22655306903948]
Robot Arm 1 togoal: 19.5605808005294
Mobile Platform 2 Pose: [-2.36220819144;9.74409002070728;-6.79838368705759]
Robot Arm 2 end: [9.11403542213892;-6.78246549584143;0.227514216362458]
Robot Arm 1 togoal: 19.4260645493807

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.780191863058357;-9.74412405111597;6.10746747664733]
Robot Arm 1 end: [-9.11017654421778;6.10596152676268;0.23236479055617]
Robot Arm 1 togoal: 19.5454077441542
Mobile Platform 2 Pose: [-2.36290941769448;9.74215855654353;-6.84444593148619]
Robot Arm 2 end: [9.10929421617344;-6.82852401917466;0.231814593368233]
Robot Arm 1 togoal: 19.4135361114328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.78019174086909;-9.74225476031884;6.15290698327274]
Robot Arm 1 end: [-9.10721251602971;6.15133123778969;0.234043104013246]
Robot Arm 1 togoal: 19.5334093139437
Mobile Platform 2 Pose: [-2.36326842611371;9.74022503140794;-6.88990489098375]
Robot Arm 2 end: [9.10657474888723;-6.87402624591267;0.233037929601382]
Robot Arm 1 togoal: 19.4033954977175

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.780191619171791;-9.74038429479141;6.19781895575265]
Robot Arm 1 end: [-9.10504135608257;6.19617816892893;0.234512776541579]
Robot Arm 1 togoal: 19.5224669321344
Mobile Platform 2 Pose: [-2.36328275681938;9.7382903508479;-6.93478148719003]
Robot Arm 2 end: [9.10443363062309;-6.91896207789327;0.233364951300845]
Robot Arm 1 togoal: 19.3940759780656

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.780191497785077;-9.73851297749649;6.2421988448631]
Robot Arm 1 end: [-9.10309851551358;6.24049452848355;0.234630543975212]
Robot Arm 1 togoal: 19.5119707016276
Mobile Platform 2 Pose: [-2.36295450832271;9.73635472526501;-6.97908599307096]
Robot Arm 2 end: [9.10245804680828;-6.96333296577693;0.233432133631353]
Robot Arm 1 togoal: 19.3851252541043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.780191376656262;-9.73664091419555;6.28604942269736]
Robot Arm 1 end: [-9.10122095123594;6.2842821187868;0.234646313960418]
Robot Arm 1 togoal: 19.5017411275581
Mobile Platform 2 Pose: [-2.36229006768231;9.73441830604676;-7.02282560682317]
Robot Arm 2 end: [9.10052909067019;-7.00714351964007;0.233424626609224]
Robot Arm 1 togoal: 19.3764096725305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.780191255770531;-9.73476814689379;6.32937557500885]
Robot Arm 1 end: [-9.09936145127545;6.32754553488057;0.234632877672733]
Robot Arm 1 togoal: 19.4917231156778
Mobile Platform 2 Pose: [-2.36130011456666;9.73248106542183;-7.06600607189538]
Robot Arm 2 end: [9.09861235248961;-7.05039863872301;0.233396229768972]
Robot Arm 1 togoal: 19.3678874803172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.780191135124162;-9.73289469851461;6.37218277663771]
Robot Arm 1 end: [-9.09750635206352;6.37029016457809;0.234611415628144]
Robot Arm 1 togoal: 19.4818976987989
Mobile Platform 2 Pose: [-2.36098866980273;9.73054307150961;-7.10863328348492]
Robot Arm 2 end: [9.18690175245093;-7.09537616095691;0.136873532890056]
Robot Arm 1 togoal: 19.4548154599702

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.780191014716662;-9.73102058609188;6.41447664567216]
Robot Arm 1 end: [-9.09565173572244;6.41252160031521;0.234588040497359]
Robot Arm 1 togoal: 19.4722564745269
Mobile Platform 2 Pose: [-2.35753434903952;9.72859527984365;-7.15117657613347]
Robot Arm 2 end: [9.15798496971089;-7.1371981716792;0.157018080456567]
Robot Arm 1 togoal: 19.418662906718

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.780190894548454;-9.72914582470287;6.45626281258224]
Robot Arm 1 end: [-9.09379645171686;6.45424546484595;0.234564572250574]
Robot Arm 1 togoal: 19.4627941024132
Mobile Platform 2 Pose: [-2.35452811353989;9.72666300144181;-7.19309291355786]
Robot Arm 2 end: [9.13373943397298;-7.17862936567574;0.178420476279827]
Robot Arm 1 togoal: 19.3871771220287

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.780190774620216;-9.7272704286093;6.49754688249278]
Robot Arm 1 end: [-9.09194021405091;6.49546736133407;0.234541487601255]
Robot Arm 1 togoal: 19.4535062561095
Mobile Platform 2 Pose: [-2.35208824370653;9.72473598462418;-7.2343690488017]
Robot Arm 2 end: [9.11348011699607;-7.2195911418123;0.20094792764086]
Robot Arm 1 togoal: 19.3597090131859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.780190654932669;-9.72539441162103;6.53833442304379]
Robot Arm 1 end: [-9.09008294998272;6.53619285698616;0.234518925323944]
Robot Arm 1 togoal: 19.4443889211442
Mobile Platform 2 Pose: [-2.35032324261219;9.72280400297011;-7.27499388618017]
Robot Arm 2 end: [9.09533127297167;-7.25996778549828;0.223842042951109]
Robot Arm 1 togoal: 19.3344788325564

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.780190535486528;-9.72351778719938;6.5786309610231]
Robot Arm 1 end: [-9.0882246567083;6.57642747864054;0.234496914409664]
Robot Arm 1 togoal: 19.4354382390315
Mobile Platform 2 Pose: [-2.34932216415503;9.72086491410684;-7.31496726590123]
Robot Arm 2 end: [9.08887422099343;-7.2999234688198;0.230583703700669]
Robot Arm 1 togoal: 19.3220264598953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.780190416282481;-9.72164056849283;6.6184419810156]
Robot Arm 1 end: [-9.08636533981663;6.61617671103068;0.234475466974553]
Robot Arm 1 togoal: 19.4266504397954
Mobile Platform 2 Pose: [-2.34914468858129;9.71892350754021;-7.35438638759987]
Robot Arm 2 end: [9.08565121871527;-7.33938098111444;0.232533576910433]
Robot Arm 1 togoal: 19.3132369562829

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.780190297321193;-9.71976276835118;6.65777292489743]
Robot Arm 1 end: [-9.08450502011346;6.65544599630094;0.234454570041995]
Robot Arm 1 togoal: 19.4180218480086
Mobile Platform 2 Pose: [-2.34981351016062;9.7169809854693;-7.39328638216869]
Robot Arm 2 end: [9.08335350011133;-7.37832835293948;0.23308008962725]
Robot Arm 1 togoal: 19.3056023146326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.780190178603296;-9.71788439933896;6.69662919128672]
Robot Arm 1 end: [-9.08264371868753;6.69424073314226;0.234434221061094]
Robot Arm 1 togoal: 19.4095488646263
Mobile Platform 2 Pose: [-2.35131051192036;9.71503765665444;-7.4316829752806]
Robot Arm 2 end: [9.08132282153266;-7.41676832914163;0.233217293933113]
Robot Arm 1 togoal: 19.2984028039347

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.780190060129396;-9.71600547372443;6.73501613532931]
Robot Arm 1 end: [-9.08078147038064;6.73256627707333;0.234414383583376]
Robot Arm 1 togoal: 19.4012279818188
Mobile Platform 2 Pose: [-2.35165903261495;9.71309366589742;-7.46958629740248]
Robot Arm 2 end: [9.16892385511573;-7.45688105064954;0.136665317748292]
Robot Arm 1 togoal: 19.3863735202398

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.780189941900068;-9.71412600350874;6.7729390681354]
Robot Arm 1 end: [-9.07891829389712;6.77042793943135;0.234395046630076]
Robot Arm 1 togoal: 19.3930557485313
Mobile Platform 2 Pose: [-2.35164816973256;9.71114135439498;-7.50752125097282]
Robot Arm 2 end: [9.14010524224956;-7.49408461905871;0.156636787094427]
Robot Arm 1 togoal: 19.3516078804206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.780189823915856;-9.71224600042338;6.81040325676125]
Robot Arm 1 end: [-9.0770541954689;6.8078309872539;0.23437622607321]
Robot Arm 1 togoal: 19.3850287727005
Mobile Platform 2 Pose: [-2.35163726916588;9.70920556592717;-7.54487825563149]
Robot Arm 2 end: [9.11643271353049;-7.53097176851795;0.17796690778586]
Robot Arm 1 togoal: 19.3219522301856

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.780189706177284;-9.71036547591873;6.84741392422626]
Robot Arm 1 end: [-9.07518919365527;6.8447806437883;0.234357908852812]
Robot Arm 1 togoal: 19.37714374799
Mobile Platform 2 Pose: [-2.35162632393249;9.70727380520104;-7.58164083702473]
Robot Arm 2 end: [9.09640977559431;-7.56742174351756;0.2003589786139]
Robot Arm 1 togoal: 19.2959683871707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.780189588684848;-9.70848444118832;6.88397624906858]
Robot Arm 1 end: [-9.07332332885493;6.88128208789686;0.234340052450182]
Robot Arm 1 togoal: 19.3693974620519
Mobile Platform 2 Pose: [-2.35161533063425;9.70533668016048;-7.61779771631772]
Robot Arm 2 end: [9.07827864142345;-7.60332987262078;0.223246702427879]
Robot Arm 1 togoal: 19.2719720641305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.780189471439012;-9.7066029071762;6.92009536491324]
Robot Arm 1 end: [-9.07145660714448;6.91734045332829;0.234322661786482]
Robot Arm 1 togoal: 19.3617867349982
Mobile Platform 2 Pose: [-2.35160430008817;9.70339240860042;-7.65334694873398]
Robot Arm 2 end: [9.07156882752988;-7.63885005966044;0.230379564092892]
Robot Arm 1 togoal: 19.2604638493873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.780189354440218;-9.70472088457571;6.95577636075795]
Robot Arm 1 end: [-9.06958904577518;6.95296082906092;0.234305737256666]
Robot Arm 1 togoal: 19.3543084663277
Mobile Platform 2 Pose: [-2.35159321078105;9.7014457714081;-7.68839013099894]
Robot Arm 2 end: [9.06826943600382;-7.6739288062639;0.232445655654924]
Robot Arm 1 togoal: 19.2527888572201

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.780189237688884;-9.70283838381999;6.9910242808066]
Robot Arm 1 end: [-9.06772069122913;6.98814825972948;0.234289221833821]
Robot Arm 1 togoal: 19.3469596549625
Mobile Platform 2 Pose: [-2.35158207068864;9.69949812199005;-7.72296426650441]
Robot Arm 2 end: [9.06594351989032;-7.70855664341581;0.233029105620882]
Robot Arm 1 togoal: 19.2462927335331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.780189121185398;-9.70095541511646;7.02584412386087]
Robot Arm 1 end: [-9.06585151595979;7.0229077439424;0.234273171117347]
Robot Arm 1 togoal: 19.339737285234
Mobile Platform 2 Pose: [-2.35157084551753;9.69754983803831;-7.75708421517523]
Robot Arm 2 end: [9.0638979620668;-7.74273536412313;0.233180731932283]
Robot Arm 1 togoal: 19.2402164128079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.780189004930132;-9.69907198842349;7.0602408442638]
Robot Arm 1 end: [-9.06398156294663;7.05724423623727;0.234257543876013]
Robot Arm 1 togoal: 19.3326384817469
Mobile Platform 2 Pose: [-2.35155959965907;9.69560097185708;-7.79075794028642]
Robot Arm 2 end: [9.06193283091671;-7.7764692068856;0.233207079405616]
Robot Arm 1 togoal: 19.2343386432869

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.781503762432456;-9.69718810968418;7.0942192827514]
Robot Arm 1 end: [-9.0621107870579;7.09117715197427;0.234242306456188]
Robot Arm 1 togoal: 19.3256581616554
Mobile Platform 2 Pose: [-2.35154829768982;9.69365169106176;-7.82399191679685]
Robot Arm 2 end: [9.05999039896126;-7.80976358227991;0.233197451698236]
Robot Arm 1 togoal: 19.2285934217096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.781768126151063;-9.69530381542329;7.127781643374]
Robot Arm 1 end: [-9.14850720444036;7.12503832338945;0.139162379890783]
Robot Arm 1 togoal: 19.4123725497125
Mobile Platform 2 Pose: [-2.35153692444191;9.69170193955302;-7.85679139756867]
Robot Arm 2 end: [9.05805387993283;-7.84262343930862;0.233177764204287]
Robot Arm 1 togoal: 19.2229596427307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.785308341983643;-9.69341891111502;7.16003406504777]
Robot Arm 1 end: [-9.11802453754928;7.15729759907255;0.161085798891017]
Robot Arm 1 togoal: 19.3759950916838
Mobile Platform 2 Pose: [-2.35152550861724;9.68975182359797;-7.88916178401038]
Robot Arm 2 end: [9.05611855378352;-7.87505409292344;0.23315554789319]
Robot Arm 1 togoal: 19.2174294008506

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.78843764624406;-9.69154551154777;7.19210652190839]
Robot Arm 1 end: [-9.09314466047897;7.18925031883868;0.184357664410577]
Robot Arm 1 togoal: 19.345157004517
Mobile Platform 2 Pose: [-2.35151402197168;9.68780127554117;-7.92110836847461]
Robot Arm 2 end: [9.05418295175807;-7.9070608109931;0.233132989774749]
Robot Arm 1 togoal: 19.2119984344153

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.791041078617692;-9.6896731097101;7.22400737701823]
Robot Arm 1 end: [-9.0721855432488;7.22097148114005;0.208703594021667]
Robot Arm 1 togoal: 19.3182137570193
Mobile Platform 2 Pose: [-2.35150248105818;9.68585032250773;-7.95263618550692]
Robot Arm 2 end: [9.05224670090651;-7.93864862101574;0.233110721664732]
Robot Arm 1 togoal: 19.2066637564439

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.793000569719365;-9.68779425651597;7.25573931934996]
Robot Arm 1 end: [-9.05768539501642;7.25255178045848;0.226811103257651]
Robot Arm 1 togoal: 19.2981545551624
Mobile Platform 2 Pose: [-2.35149089796923;9.68389900006759;-7.98375034593945]
Robot Arm 2 end: [9.05030975329083;-7.96982263226397;0.233088873664418]
Robot Arm 1 togoal: 19.2014227834849

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.794210850368582;-9.68590986260607;7.28724746255886]
Robot Arm 1 end: [-9.05225571549751;7.2839811174883;0.232131317525075]
Robot Arm 1 togoal: 19.2879856102065
Mobile Platform 2 Pose: [-2.35147924673967;9.68194723415692;-8.01445597037027]
Robot Arm 2 end: [9.04837198037853;-8.00058796500765;0.233067544968787]
Robot Arm 1 togoal: 19.1962729090119

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.794592202635626;-9.68402373221073;7.31841819953098]
Robot Arm 1 end: [-9.04935807673184;7.31508700340815;0.233676074320659]
Robot Arm 1 togoal: 19.2806639881629
Mobile Platform 2 Pose: [-2.35146755671385;9.67999512425553;-8.04475800179433]
Robot Arm 2 end: [9.0464335180277;-8.03094956252436;0.233046674937619]
Robot Arm 1 togoal: 19.1912118728733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.794102542637435;-9.68213690100018;7.3492219941606]
Robot Arm 1 end: [-9.0471865657278;7.34582315549814;0.234117120029025]
Robot Arm 1 togoal: 19.2742324967376
Mobile Platform 2 Pose: [-2.35178736521677;9.67804265343144;-8.07466128655872]
Robot Arm 2 end: [9.14629988627769;-8.06314827115355;0.124518976409199]
Robot Arm 1 togoal: 19.2946197994172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.792746366184724;-9.68024953073683;7.37965511611771]
Robot Arm 1 end: [-9.04522474183319;7.37618116897679;0.234236385156372]
Robot Arm 1 togoal: 19.2681283814867
Mobile Platform 2 Pose: [-2.3541211131063;9.67608290604781;-8.10487339933832]
Robot Arm 2 end: [9.11721927744392;-8.09262749588745;0.143776424736519]
Robot Arm 1 togoal: 19.2614352411127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.792731266671103;-9.67836174568871;7.40972078299298]
Robot Arm 1 end: [-9.04332337979571;7.40618735434607;0.234261867569827]
Robot Arm 1 togoal: 19.2621837912963
Mobile Platform 2 Pose: [-2.35625212273169;9.67414114108989;-8.13458810693739]
Robot Arm 2 end: [9.09251768677104;-8.12184327428738;0.164422040702648]
Robot Arm 1 togoal: 19.2325767401473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.792716323845959;-9.67647360104506;7.43941970044747]
Robot Arm 1 end: [-9.04143871319649;7.43582709824477;0.23426085558489]
Robot Arm 1 togoal: 19.2563513824474
Mobile Platform 2 Pose: [-2.35813793732546;9.67220631125092;-8.16378956726873]
Robot Arm 2 end: [9.07180890069041;-8.15072624772476;0.186323907555864]
Robot Arm 1 togoal: 19.2076825366063

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.792701483710267;-9.67458509924179;7.46875579088845]
Robot Arm 1 end: [-9.03955846695288;7.46510420040664;0.234252344797981]
Robot Arm 1 togoal: 19.2506156488431
Mobile Platform 2 Pose: [-2.35974450087906;9.6702667485219;-8.19246270061233]
Robot Arm 2 end: [9.05310748962679;-8.17915166622123;0.208797067908862]
Robot Arm 1 togoal: 19.1848448052333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.792686779864044;-9.67269621623963;7.49773354514773]
Robot Arm 1 end: [-9.0376790659284;7.4940231157763;0.234241857831583]
Robot Arm 1 togoal: 19.2449705346095
Mobile Platform 2 Pose: [-2.36104521195487;9.66831959654114;-8.22060314532611]
Robot Arm 2 end: [9.03932430178347;-8.20713996130741;0.226109429692683]
Robot Arm 1 togoal: 19.1673409364965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.792672189246928;-9.67080699203614;7.52635697973144]
Robot Arm 1 end: [-9.03579950283861;7.52258784950208;0.234231036394609]
Robot Arm 1 togoal: 19.2394128819308
Mobile Platform 2 Pose: [-2.36202097462897;9.66636693007131;-8.24825075987378]
Robot Arm 2 end: [9.0340600490916;-8.23478109649015;0.231170173155854]
Robot Arm 1 togoal: 19.1592040730285

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.792657712997491;-9.66891754723844;7.55463053222484]
Robot Arm 1 end: [-9.03391961965853;7.55080283631188;0.234220309257579]
Robot Arm 1 togoal: 19.2339404388895
Mobile Platform 2 Pose: [-2.36265882762501;9.66441249990246;-8.27549761206487]
Robot Arm 2 end: [9.03116573771353;-8.26206409090195;0.232629761460806]
Robot Arm 1 togoal: 19.1537356260341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.792643424406279;-9.66702779657173;7.58255849080084]
Robot Arm 1 end: [-9.03203926545024;7.5786723642602;0.234209788338458]
Robot Arm 1 togoal: 19.2285510418769
Mobile Platform 2 Pose: [-2.36295174495455;9.66245734241;-8.3023741466921]
Robot Arm 2 end: [9.02894817026966;-8.28899144220759;0.233040420764217]
Robot Arm 1 togoal: 19.1490798376683

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.79262930520888;-9.6651376637353;7.61014453903154]
Robot Arm 1 end: [-9.03015833532892;7.6062001162674;0.234199522063005]
Robot Arm 1 togoal: 19.2232426926642
Mobile Platform 2 Pose: [-2.36289755973635;9.6605017919678;-8.32889175875628]
Robot Arm 2 end: [9.02692489000354;-8.31556668851835;0.233146740850868]
Robot Arm 1 togoal: 19.1447055962678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.792615318389014;-9.66324722324279;7.63739304507496]
Robot Arm 1 end: [-9.02827689549057;7.63339046001431;0.23418951832103]
Robot Arm 1 togoal: 19.2180135170618
Mobile Platform 2 Pose: [-2.36249880260675;9.65854585792043;-8.35505611202013]
Robot Arm 2 end: [9.02495697365854;-8.34179322420342;0.233165196071068]
Robot Arm 1 togoal: 19.1404586163202

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.792601668790668;-9.66135647378908;7.66430796630757]
Robot Arm 1 end: [-9.02639494099783;7.66024735487986;0.234179790344787]
Robot Arm 1 togoal: 19.2128616593976
Mobile Platform 2 Pose: [-2.36215225900916;9.65658975157656;-8.38087183711279]
Robot Arm 2 end: [9.11388064764404;-8.36965310881702;0.136163822838315]
Robot Arm 1 togoal: 19.2332065874504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.792588181509906;-9.6594654224598;7.6908933231137]
Robot Arm 1 end: [-9.0245125175195;7.68677481966329;0.234170277754183]
Robot Arm 1 togoal: 19.2077853486094
Mobile Platform 2 Pose: [-2.35814932955311;9.65462252314104;-8.40703060951838]
Robot Arm 2 end: [9.08545201382055;-8.39516705046536;0.155759842249186]
Robot Arm 1 togoal: 19.2014022419222

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.792574964997007;-9.65757404476161;7.71715346661059]
Robot Arm 1 end: [-9.02262956626472;7.71297720630975;0.234161028462697]
Robot Arm 1 togoal: 19.2027826997249
Mobile Platform 2 Pose: [-2.35437789693159;9.65267206455684;-8.43270888091739]
Robot Arm 2 end: [9.06138705232903;-8.42043613760064;0.176591579710407]
Robot Arm 1 togoal: 19.1739128076311

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
{函数或变量 'Je' 无法识别。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 17)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',17,0)">第 17 行</a>)
    X_d = pinv(Je)*Ve;

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 101)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',101,0)">第 101 行</a>)
    [dx1] = mobile_robot_CBLF(mobile_robot1, target1, a1_G, obstacle_results1, D_min, V_max, dt);
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',17,0)
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.051621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
{无法识别的字段名称 "Kp"。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 17)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',17,0)">第 17 行</a>)
    Ve = FeedbackControl(a_k ,a_k_next,a_dk,mobile_robot.Kp,mobile_robot.Ki,dt);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 107)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',107,0)">第 107 行</a>)
    [dx2] = mobile_robot_CBLF(mobile_robot2, target2, a2_G, obstacle_results2, D_min, V_max, dt);
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',17,0)
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.051621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.052165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785389464117813;-9.90082571744752;2.36162035136777]
Robot Arm 1 end: [-9.16528507971623;2.36373825415995;1.02051715343198]
Robot Arm 1 togoal: 20.6366452873882
Mobile Platform 2 Pose: [-2.35688857419555;9.90082571714099;-2.36162035166892]
Robot Arm 2 end: [9.16561981275206;-2.33933148857434;1.02051715343198]
Robot Arm 1 togoal: 20.645999804529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.472815e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.458659e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785385021347842;-9.82700776114751;1.73789065374232]
Robot Arm 1 end: [-10.0025002904493;1.73814003361593;-0.0599881444521516]
Robot Arm 1 togoal: 21.6992596990418
Mobile Platform 2 Pose: [-2.35575836765222;9.80068672901245;-1.19431046280483]
Robot Arm 2 end: [9.36042493540518;-1.18494637140319;0.0572833747322158]
Robot Arm 1 togoal: 21.3230788235091

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.347331e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.610178e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785390692653753;-9.72384331821644;6.05793978126748]
Robot Arm 1 end: [-10.5236500555391;6.05563557354325;0.839020281506679]
Robot Arm 1 togoal: 20.9103639056656
Mobile Platform 2 Pose: [-2.35575333007842;9.69987845513388;-7.02092847065603]
Robot Arm 2 end: [10.2523427112052;-7.03123983461092;0.1342683174614]
Robot Arm 1 togoal: 20.5157000117243

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785387977504445;-9.67765927876204;6.60741313647127]
Robot Arm 1 end: [-9.79781750366111;6.60741944407666;1.51160365928395]
Robot Arm 1 togoal: 20.086395122767
Mobile Platform 2 Pose: [-2.35575700224035;9.6528830087122;-6.61519090903274]
Robot Arm 2 end: [10.329134550203;-6.63371871617607;1.15876204486374]
Robot Arm 1 togoal: 20.6091430578329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.251160e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.447049e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785392672589808;-9.5822808245283;8.46002857430585]
Robot Arm 1 end: [-9.00208186336998;8.46008732092635;1.25765310496476]
Robot Arm 1 togoal: 19.0661951576215
Mobile Platform 2 Pose: [-2.35497520401104;9.55446948251385;-7.77810494261156]
Robot Arm 2 end: [9.37879718279327;-7.77558843877103;1.49581405125204]
Robot Arm 1 togoal: 19.5060611446181

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.660112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.573430e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785390212920382;-9.53783734971316;8.97505582639182]
Robot Arm 1 end: [-8.77766238201497;8.96897395633102;0.516230844191018]
Robot Arm 1 togoal: 18.8327687898548
Mobile Platform 2 Pose: [-2.35455782143732;9.46554288867806;-7.99076988192002]
Robot Arm 2 end: [8.71382421554173;-7.97502548489798;0.949167988967034]
Robot Arm 1 togoal: 18.8317479381456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.354433e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.348074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.785387058235842;-9.49259957316412;9.57367674894873]
Robot Arm 1 end: [-9.34956054811836;9.56788844347117;-0.0596725101848627]
Robot Arm 1 togoal: 19.4188243564599
Mobile Platform 2 Pose: [-2.35455526203394;9.42079233068586;-8.53010923144469]
Robot Arm 2 end: [8.85030846200575;-8.52435281866369;0.179867985245073]
Robot Arm 1 togoal: 18.9554820249328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.917808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.957080e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.78538532269393;-9.40706287794072;9.34807943481004]
Robot Arm 1 end: [-10.1619427475043;9.35621222436207;0.447442398121563]
Robot Arm 1 togoal: 20.2007654305042
Mobile Platform 2 Pose: [-2.35455232281999;9.37463865875801;-9.03335719184155]
Robot Arm 2 end: [9.7277486918307;-9.04177673083835;0.00700015520208891]
Robot Arm 1 togoal: 19.8089488820492

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.776453e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.644609e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785390701953156;-9.36066145200561;9.70436844936195]
Robot Arm 1 end: [-9.8271144756011;9.70314019168788;1.36564924653874]
Robot Arm 1 togoal: 19.8299452351988
Mobile Platform 2 Pose: [-2.3545565395444;9.32780802083269;-9.77835743058682]
Robot Arm 2 end: [10.1018452724804;-9.79698137057846;0.917497712937763]
Robot Arm 1 togoal: 20.111927180982

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.813605e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.619275e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785387737542833;-9.3159803995015;10.3689080143335]
Robot Arm 1 end: [-9.00806168434338;10.3623699199653;1.43937637810784]
Robot Arm 1 togoal: 19.0116907025743
Mobile Platform 2 Pose: [-2.35455587641611;9.2823417982714;-9.28987015276913]
Robot Arm 2 end: [9.41810310094327;-9.29601471987985;1.49509151496215]
Robot Arm 1 togoal: 19.4308773492883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.332611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.316189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785389739201768;-9.27175526204193;10.7301393428517]
Robot Arm 1 end: [-8.51412094732085;10.7233832717345;0.869208736576266]
Robot Arm 1 togoal: 18.5397084567402
Mobile Platform 2 Pose: [-2.3574217070332;9.13658333218907;-9.39311108562842]
Robot Arm 2 end: [8.56698030723435;-9.38226404620308;1.24205205149559]
Robot Arm 1 togoal: 18.5793478732655

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.444713e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.459837e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.789359387418411;-9.12393615398514;10.7780371358862]
Robot Arm 1 end: [-8.58557234170826;10.7660509608573;0.168225950813991]
Robot Arm 1 togoal: 18.650478036009
Mobile Platform 2 Pose: [-2.35964450988065;9.0437701698148;-9.19112709810528]
Robot Arm 2 end: [8.2837480366534;-9.18197035328085;0.630653431089775]
Robot Arm 1 togoal: 18.3236822659505

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.524474e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.546950e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.791379152487286;-9.07302411639183;10.4935636290245]
Robot Arm 1 end: [-9.33291959789291;10.4897878011985;-0.0133255091217365]
Robot Arm 1 togoal: 19.3998924491023
Mobile Platform 2 Pose: [-2.35964195566183;8.99979007878766;-9.53427606344539]
Robot Arm 2 end: [8.61867114429847;-9.537772607952;0.0556790236771148]
Robot Arm 1 togoal: 18.6819628226036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.587068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.600490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.791381470160211;-9.02671578681752;10.0624217849248]
Robot Arm 1 end: [-9.80671097452484;10.0655264336377;0.758304757477433]
Robot Arm 1 togoal: 19.8214983635152
Mobile Platform 2 Pose: [-2.35963948691095;8.95442161125928;-9.84815217730693]
Robot Arm 2 end: [9.40446006970598;-9.86104991337804;0.0915409999924138]
Robot Arm 1 togoal: 19.4575366055441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.697231e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.667130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.791383785413627;-8.98152270262549;10.5401249195354]
Robot Arm 1 end: [-9.29437027798525;10.5369774481293;1.42236291656146]
Robot Arm 1 togoal: 19.302093111319
Mobile Platform 2 Pose: [-2.35964422660056;8.90839444254664;-10.3420244919547]
Robot Arm 2 end: [9.64878551192541;-10.3619845498459;0.936751413585909]
Robot Arm 1 togoal: 19.6608024078038

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.795770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.775291e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.791384253775427;-8.93750816043857;10.8933682157597]
Robot Arm 1 end: [-8.57124321646703;10.8862125474029;1.37773392325157]
Robot Arm 1 togoal: 18.5929280225868
Mobile Platform 2 Pose: [-2.35964366554152;8.86383221225032;-9.71532577594871]
Robot Arm 2 end: [9.07575950980666;-9.72580755979088;1.44797474727197]
Robot Arm 1 togoal: 19.0778697725344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.871635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.857002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.790899273730948;-8.82575279306216;11.0597220962699]
Robot Arm 1 end: [-8.10433197380787;11.0494778718969;0.888669307192943]
Robot Arm 1 togoal: 18.1457455654627
Mobile Platform 2 Pose: [-2.36032305040813;8.72136267347062;-9.60492276150076]
Robot Arm 2 end: [8.29978640335071;-9.59865708421576;1.33039504798137]
Robot Arm 1 togoal: 18.3051792887162

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.930886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.938449e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.790220091901088;-8.69751905399707;10.9246525130685]
Robot Arm 1 end: [-8.07722128272867;10.9092934417878;0.325020827292596]
Robot Arm 1 togoal: 18.1395454752749
Mobile Platform 2 Pose: [-2.36098947769272;8.64414464829536;-9.37681499633157]
Robot Arm 2 end: [7.94023535190107;-9.37006749466397;0.914757437812283]
Robot Arm 1 togoal: 17.9615252550302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.969327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.984940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.789352822748242;-8.5759196882923;10.7758195507773]
Robot Arm 1 end: [-8.47994007350881;10.7616827764622;-0.00996906565823669]
Robot Arm 1 togoal: 18.5588849894717
Mobile Platform 2 Pose: [-2.36202328475102;8.57321130537732;-9.61683586446767]
Robot Arm 2 end: [7.92008674843634;-9.6144964904323;0.408724424372857]
Robot Arm 1 togoal: 17.9587103931076

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.996028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.017949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.789351662801713;-8.53133754176915;10.392033939754]
Robot Arm 1 end: [-9.10179835651269;10.3907634263055;0.249568079789062]
Robot Arm 1 togoal: 19.1480530507919
Mobile Platform 2 Pose: [-2.36433520742949;8.4293197652103;-9.59434051875884]
Robot Arm 2 end: [8.1973764235239;-9.60475278370376;0.0360743744873673]
Robot Arm 1 togoal: 18.2621393372934

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.030765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.045428e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.788248330946204;-8.4614919653382;10.1430990921734]
Robot Arm 1 end: [-9.14121317774235;10.1410172211302;0.99488515738872]
Robot Arm 1 togoal: 19.1489614496465
Mobile Platform 2 Pose: [-2.36433320996706;8.3854267040345;-9.99807787562474]
Robot Arm 2 end: [8.78158572006748;-10.015109695859;0.114155386475674]
Robot Arm 1 togoal: 18.8342082984593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.096450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.068890e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.788248444176074;-8.41786717725515;10.7214651537605]
Robot Arm 1 end: [-8.6521302030443;10.7149338892879;1.39892690916864]
Robot Arm 1 togoal: 18.6662260089412
Mobile Platform 2 Pose: [-2.36433623407863;8.34068455649843;-10.3101205828236]
Robot Arm 2 end: [9.06073129902803;-10.330829036323;0.747479721247453]
Robot Arm 1 togoal: 19.0792887479241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.142293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.120960e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.787057051552023;-8.29018390889141;10.8572583110663]
Robot Arm 1 end: [-7.99324705661954;10.8508567805687;1.35457380034297]
Robot Arm 1 togoal: 18.014122087449
Mobile Platform 2 Pose: [-2.36433835426182;8.29678384773013;-9.85696360349053]
Robot Arm 2 end: [8.71686784948568;-9.87383943996971;1.28879384057072]
Robot Arm 1 togoal: 18.7187333514449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.195155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.171367e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785856591809468;-8.20935600592155;11.0765496740423]
Robot Arm 1 end: [-7.62655788460674;11.06464483445;1.08285892050083]
Robot Arm 1 togoal: 17.6641155761747
Mobile Platform 2 Pose: [-2.36561894050362;8.23511899069658;-9.59890057389446]
Robot Arm 2 end: [8.15941834469477;-9.60481366387237;1.40558196431362]
Robot Arm 1 togoal: 18.1640845676533

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.226331e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.215940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.784684497503982;-8.13061998582729;10.8539059732606]
Robot Arm 1 end: [-7.45556877818845;10.841559187766;0.72424007141149]
Robot Arm 1 togoal: 17.4939969595397
Mobile Platform 2 Pose: [-2.36683847890724;8.12130788764827;-9.4402271852072]
Robot Arm 2 end: [7.67692442850438;-9.43772721504913;1.22626425442498]
Robot Arm 1 togoal: 17.6883203582327

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.253149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.253632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.783559532564675;-8.00066621660097;10.9812655701258]
Robot Arm 1 end: [-7.42711702843306;10.9638830441033;0.380558164145011]
Robot Arm 1 togoal: 17.4909715575316
Mobile Platform 2 Pose: [-2.36797210634607;7.9872187172266;-9.55930221536008]
Robot Arm 2 end: [7.36603989765946;-9.55353124057769;0.943401478811875]
Robot Arm 1 togoal: 17.3813778548169

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.274971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.284201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.783557207508831;-7.95897099653154;10.6669785429104]
Robot Arm 1 end: [-7.65787030546306;10.6471073542011;0.132103614051118]
Robot Arm 1 togoal: 17.7242253659594
Mobile Platform 2 Pose: [-2.37002241424146;7.8507825734853;-9.50968901592798]
Robot Arm 2 end: [7.20489729827219;-9.50666643793448;0.657883910872211]
Robot Arm 1 togoal: 17.233596212946

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.285218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.308589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.781670755443701;-7.81726673342919;10.633673383402]
Robot Arm 1 end: [-7.95747874593769;10.6187281797422;0.0774252870901816]
Robot Arm 1 togoal: 18.0260304954519
Mobile Platform 2 Pose: [-2.37091791345703;7.77310042297859;-9.73197367549336]
Robot Arm 2 end: [7.21662987508526;-9.73523884920176;0.405428344510765]
Robot Arm 1 togoal: 17.254765800821

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.301613e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.324645e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.781670925563323;-7.77436842882214;10.2861190235208]
Robot Arm 1 end: [-8.30855601309984;10.2805293272807;0.342801120289664]
Robot Arm 1 togoal: 18.3485714045652
Mobile Platform 2 Pose: [-2.37242626294715;7.6360805872528;-9.67908229098806]
Robot Arm 2 end: [7.2856456637757;-9.69067811384592;0.194859325961337]
Robot Arm 1 togoal: 17.3392005331907

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.317826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.341636e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.781677318006795;-7.67717539153597;10.4791616606032]
Robot Arm 1 end: [-8.31184028148054;10.4750341273844;0.832188360167105]
Robot Arm 1 togoal: 18.3309468765747
Mobile Platform 2 Pose: [-2.37291679715699;7.59399252292542;-9.96906382955331]
Robot Arm 2 end: [7.53621259029213;-9.98757835857726;0.0978748748416019]
Robot Arm 1 togoal: 17.593868006523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.349096e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.349401e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.781681636517861;-7.63096810636798;10.7646443528988]
Robot Arm 1 end: [-8.05012712104824;10.7577510875965;1.18903555349335]
Robot Arm 1 togoal: 18.0690751337886
Mobile Platform 2 Pose: [-2.37379354256538;7.45373576298272;-9.96268499271267]
Robot Arm 2 end: [7.76283371704459;-9.98362377211051;0.182849315468923]
Robot Arm 1 togoal: 17.8131742565643

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.375857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.366320e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.781685363820906;-7.53388130406879;10.9539088834696]
Robot Arm 1 end: [-7.62719355325052;10.945643685537;1.32364971881559]
Robot Arm 1 togoal: 17.6536438640083
Mobile Platform 2 Pose: [-2.37194001419997;7.40393256652336;-10.2407221643733]
Robot Arm 2 end: [7.96236190083847;-10.2617848606502;0.468812554942889]
Robot Arm 1 togoal: 17.9950569543276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.405303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.382267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.78168674722294;-7.39747061633321;11.0159407928567]
Robot Arm 1 end: [-7.2339643374078;11.0070406492639;1.29033439915128]
Robot Arm 1 togoal: 17.2649026719449
Mobile Platform 2 Pose: [-2.37194253934233;7.36163431365083;-9.89101781772067]
Robot Arm 2 end: [7.94783640936284;-9.91247789935985;0.84984925591946]
Robot Arm 1 togoal: 17.9605939550069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.429393e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.404637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.781688740660634;-7.26347327954435;10.9265078296291]
Robot Arm 1 end: [-6.93655627103807;10.9181529439948;1.19710248927027]
Robot Arm 1 togoal: 16.9645174628712
Mobile Platform 2 Pose: [-2.36990666244694;7.23866798472884;-9.74977629794765]
Robot Arm 2 end: [7.64941496602749;-9.76842701073605;1.13549466281636]
Robot Arm 1 togoal: 17.6551434165058

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.438461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.432751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.781687938209902;-7.13202255778767;10.9368684886966]
Robot Arm 1 end: [-6.83683210074178;10.9277653787742;1.21410742778096]
Robot Arm 1 togoal: 16.8651666709079
Mobile Platform 2 Pose: [-2.3678415207805;7.18026970925219;-9.49155172376322]
Robot Arm 2 end: [7.3817014693814;-9.50538157686778;1.24963371243414]
Robot Arm 1 togoal: 17.3908548673498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.461091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.437690e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.781690093233911;-7.02050589987873;10.7839415206697]
Robot Arm 1 end: [-6.87029630856435;10.7769908670592;1.28680362223917]
Robot Arm 1 togoal: 16.8898034414143
Mobile Platform 2 Pose: [-2.36529581447598;7.04813052788797;-9.6092354431464]
Robot Arm 2 end: [7.18674354851508;-9.62114824586866;1.25472327381308]
Robot Arm 1 togoal: 17.1929807115852

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.464864e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.466323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.781689351243814;-6.90614649954964;10.9335262891759]
Robot Arm 1 end: [-6.83896312164487;10.9259162437591;1.30590860595197]
Robot Arm 1 togoal: 16.8657719719664
Mobile Platform 2 Pose: [-2.36331956376453;6.93808588647824;-9.45236435697238]
Robot Arm 2 end: [7.19742830287392;-9.46687208970885;1.22292348425493]
Robot Arm 1 togoal: 17.208271708991

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.482542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.466579e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.78169124340998;-6.84387287810074;10.6888292734866]
Robot Arm 1 end: [-6.9346893944212;10.6823977478238;1.31229946510014]
Robot Arm 1 togoal: 16.9497176225748
Mobile Platform 2 Pose: [-2.36147391085323;6.86249252541809;-9.67605209922361]
Robot Arm 2 end: [7.13215263006948;-9.69090257269682;1.21618719814302]
Robot Arm 1 togoal: 17.1376517003859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.487947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.487018e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.781689501718845;-6.77776785380486;10.9277316792091]
Robot Arm 1 end: [-6.97698309842754;10.9204531540409;1.29182566981183]
Robot Arm 1 togoal: 17.0034617065672
Mobile Platform 2 Pose: [-2.3598159559823;6.7543141662099;-9.51526298942084]
Robot Arm 2 end: [7.12367748193636;-9.53249218094748;1.16278026921131]
Robot Arm 1 togoal: 17.1338032976271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.499546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.495258e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.781690363414885;-6.73880133948011;10.6305276351421]
Robot Arm 1 end: [-7.06059286756655;10.6238642296662;1.24413310833413]
Robot Arm 1 togoal: 17.0742405642424
Mobile Platform 2 Pose: [-2.35841131540729;6.68688187613781;-9.75237962450391]
Robot Arm 2 end: [7.11966220797657;-9.77144344607776;1.10878659336107]
Robot Arm 1 togoal: 17.1261493659893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.507490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.507947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.781689278753274;-6.69964617792826;10.9203383654031]
Robot Arm 1 end: [-7.11641818840368;10.9133390807497;1.18331365901935]
Robot Arm 1 togoal: 17.1440949583705
Mobile Platform 2 Pose: [-2.35731833199953;6.59047364272656;-9.56978912032403]
Robot Arm 2 end: [7.10225822712262;-9.59078825094741;1.01978914047436]
Robot Arm 1 togoal: 17.1144939466071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.516440e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.518363e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.781689869483092;-6.66043304851954;10.6081338486463]
Robot Arm 1 end: [-7.1604851803809;10.6013768248808;1.10815244798822]
Robot Arm 1 togoal: 17.1759817423278
Mobile Platform 2 Pose: [-2.35658909211162;6.52232530974741;-9.80472378605292]
Robot Arm 2 end: [7.09200047083054;-9.82700489037766;0.912858280889975]
Robot Arm 1 togoal: 17.1036909394828

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.525907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.529184e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.781688891634067;-6.62122530168222;10.9018521028771]
Robot Arm 1 end: [-7.19021112580708;10.8954375867464;1.01771043354385]
Robot Arm 1 togoal: 17.2208730153057
Mobile Platform 2 Pose: [-2.3565878979239;6.43913976139317;-9.5981503605497]
Robot Arm 2 end: [7.0494965186499;-9.62113593580656;0.775540862621675]
Robot Arm 1 togoal: 17.0699905929703

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.534396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.540250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.781688886509415;-6.58197391673053;10.5944273917018]
Robot Arm 1 end: [-7.20321333479275;10.5883364536376;0.918193236659585]
Robot Arm 1 togoal: 17.2238225960151
Mobile Platform 2 Pose: [-2.35658527649874;6.37866251591585;-9.84634472870409]
Robot Arm 2 end: [6.99263245170237;-9.86884755896024;0.612216969848137]
Robot Arm 1 togoal: 17.0174218140508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.543983e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.551391e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.781687140869804;-6.54194922529295;10.8820088328736]
Robot Arm 1 end: [-7.19867103727525;10.8766530280444;0.800972459232947]
Robot Arm 1 togoal: 17.236044953511
Mobile Platform 2 Pose: [-2.35658211415832;6.30872480380605;-9.61644828928647]
Robot Arm 2 end: [6.87204017166587;-9.63732146911426;0.425097937076343]
Robot Arm 1 togoal: 16.9114835718985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.552742e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.564450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.781686470882655;-6.50282097955439;10.582388412102]
Robot Arm 1 end: [-7.1727991280967;10.5777797247401;0.672311189901042]
Robot Arm 1 togoal: 17.203462790981
Mobile Platform 2 Pose: [-2.35657760975765;6.26991954568007;-9.90282262650837]
Robot Arm 2 end: [6.69101667118166;-9.92113095297698;0.226129634664852]
Robot Arm 1 togoal: 16.7413543988312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.562605e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.577907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.781684187839087;-6.45469223958547;10.8542968273032]
Robot Arm 1 end: [-7.10727784839831;10.8505519918658;0.522980016719184]
Robot Arm 1 togoal: 17.1574601255604
Mobile Platform 2 Pose: [-2.35657183310011;6.19413005131902;-9.68674580160808]
Robot Arm 2 end: [6.3480832613651;-9.70350427748043;0.0737357346104333]
Robot Arm 1 togoal: 16.4146979834498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.572735e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.596211e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.781682742935402;-6.41598179115171;10.559630575992]
Robot Arm 1 end: [-7.00444565194219;10.5564029573467;0.355537766768233]
Robot Arm 1 togoal: 17.0534177922752
Mobile Platform 2 Pose: [-2.35656961917597;6.15782418758213;-10.0282473336884]
Robot Arm 2 end: [5.88824306762758;-10.0469110190546;0.0940504469540566]
Robot Arm 1 togoal: 15.9522617031775

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.583855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.611810e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.781678327702433;-6.35610510308293;10.8079540496776]
Robot Arm 1 end: [-6.80639161667589;10.8037322030575;0.179600721936204]
Robot Arm 1 togoal: 16.8789850598457
Mobile Platform 2 Pose: [-2.35657327888283;6.03978622247209;-10.0759592755913]
Robot Arm 2 end: [5.39383179666636;-10.091324242278;0.456174105324209]
Robot Arm 1 togoal: 15.430886289752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.597394e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622920e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.781675621924474;-6.31843795782645;10.4992348176455]
Robot Arm 1 end: [-6.50766301753388;10.4893522980041;0.0293524779331635]
Robot Arm 1 togoal: 16.5821414872177
Mobile Platform 2 Pose: [-2.3565865387378;5.92343347464929;-10.0265511595417]
Robot Arm 2 end: [5.29115645230392;-10.0299541944355;1.03700794565191]
Robot Arm 1 togoal: 15.2988434599244

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.612117e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622164e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.781669802258559;-6.19678487142408;10.6285250508637]
Robot Arm 1 end: [-5.98746166767941;10.6046039940584;0.020518509595004]
Robot Arm 1 togoal: 16.0690983340136
Mobile Platform 2 Pose: [-2.35658784508683;5.88787413873161;-9.62930619765216]
Robot Arm 2 end: [5.64226176561987;-9.63557418632857;1.3863287862709]
Robot Arm 1 togoal: 15.6470858518016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.631375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.617380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.781670058604963;-6.16132876836644;10.2554257700247]
Robot Arm 1 end: [-5.51155114084035;10.2175526168612;0.357407721752397]
Robot Arm 1 togoal: 15.5566543615909
Mobile Platform 2 Pose: [-2.35659289991828;5.77301738731357;-9.4879326683434]
Robot Arm 2 end: [5.93946162699416;-9.49922502987158;1.4126490225859]
Robot Arm 1 togoal: 15.9476942679321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.639984e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622223e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.781682572972179;-6.07261484348036;10.4410233148238]
Robot Arm 1 end: [-5.35579998702496;10.4176899661222;0.951051229192995]
Robot Arm 1 togoal: 15.3720493022453
Mobile Platform 2 Pose: [-2.35658977328789;5.65052471503849;-9.36994258501198]
Robot Arm 2 end: [6.16884539698322;-9.38915991664781;1.21216945749982]
Robot Arm 1 togoal: 16.1833267093931

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.639953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.627172e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.781683764168873;-6.03693127420211;10.8268044847432]
Robot Arm 1 end: [-5.6570120227024;10.8117094901931;1.37254233363635]
Robot Arm 1 togoal: 15.6787415775061
Mobile Platform 2 Pose: [-2.3565899630377;5.54088389571461;-9.52154605768424]
Robot Arm 2 end: [6.24743120901555;-9.54463240973065;0.857525795945049]
Robot Arm 1 togoal: 16.2673470041493

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.635178e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.635479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.781692924723572;-5.95360109259941;11.027060489007]
Robot Arm 1 end: [-5.98561256643098;11.0216953092771;1.47498161705105]
Robot Arm 1 togoal: 16.0182954186956
Mobile Platform 2 Pose: [-2.35658346123045;5.41909703010737;-9.42713117921359]
Robot Arm 2 end: [6.0830848139045;-9.44715824124527;0.421491706658444]
Robot Arm 1 togoal: 16.1301013520869

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.637484e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.649594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.781689192291312;-5.83495632065729;11.1627011631177]
Robot Arm 1 end: [-6.23608609105656;11.1589853936622;1.36061657726757]
Robot Arm 1 togoal: 16.2781897501292
Mobile Platform 2 Pose: [-2.35657721134484;5.38199466175014;-9.7098777142858]
Robot Arm 2 end: [5.65787342357077;-9.72242772968167;0.0318959225237148]
Robot Arm 1 togoal: 15.7309719174427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.641147e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.665795e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.781694079116382;-5.75144983457278;10.9613055085858]
Robot Arm 1 end: [-6.41037210050157;10.9577606071737;1.09435979798575]
Robot Arm 1 togoal: 16.4438328779986
Mobile Platform 2 Pose: [-2.35657146943941;5.27140011419661;-9.85259497522042]
Robot Arm 2 end: [4.85944639777262;-9.86212678019834;0.0831499402125683]
Robot Arm 1 togoal: 14.9294865640935

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.645814e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.683045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.781689826187366;-5.63098984569478;11.0942498885747]
Robot Arm 1 end: [-6.39182656980564;11.093992268324;0.744472763746536]
Robot Arm 1 togoal: 16.4466346687212
Mobile Platform 2 Pose: [-2.35657505243766;5.2384842533132;-10.2322302810996]
Robot Arm 2 end: [4.47993312714075;-10.2302908724669;0.852874499052309]
Robot Arm 1 togoal: 14.4971683075498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.655094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.676374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.781688405149036;-5.59414843265503;10.7943063001899]
Robot Arm 1 end: [-6.25526672663057;10.7958553073222;0.33000622255009]
Robot Arm 1 togoal: 16.3182581240162
Mobile Platform 2 Pose: [-2.35657678255139;5.20436283959464;-9.76452534285438]
Robot Arm 2 end: [4.89220617044889;-9.76580421845306;1.42320931154423]
Robot Arm 1 togoal: 14.8943685769488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.665495e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.670671e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.781678438862681;-5.47350467436434;10.9072235937171]
Robot Arm 1 end: [-5.75578266116512;10.9064931451673;-0.00425949137462905]
Robot Arm 1 togoal: 15.8553723893239
Mobile Platform 2 Pose: [-2.35658019681388;5.12705091141062;-9.55739478896652]
Robot Arm 2 end: [5.42099563109432;-9.56939291778187;1.42711765123738]
Robot Arm 1 togoal: 15.4272921346844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.684166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.671754e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.781675490297524;-5.43896980592272;10.5306943634022]
Robot Arm 1 end: [-5.02375543760081;10.5131204145316;0.0466095968195095]
Robot Arm 1 togoal: 15.1046465390501
Mobile Platform 2 Pose: [-2.3565787804833;5.02679604570836;-9.39178341773396]
Robot Arm 2 end: [5.72496979612277;-9.41187422160776;1.03116995199194]
Robot Arm 1 togoal: 15.743586011329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.697737e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.678011e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.781679109950803;-5.40556417073786;10.1935158993342]
Robot Arm 1 end: [-4.60778223699832;10.1752436013035;0.780422997035587]
Robot Arm 1 togoal: 14.6275926420133
Mobile Platform 2 Pose: [-2.35657569150298;4.90982115655606;-9.52568897559203]
Robot Arm 2 end: [5.62375335637391;-9.54351315963014;0.434893101109182]
Robot Arm 1 togoal: 15.668110204235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.694936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.691796e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.781681601210757;-5.37138532815881;10.6257515113455]
Robot Arm 1 end: [-4.97180413346205;10.6179241669773;1.41035747188875]
Robot Arm 1 togoal: 14.9849588273012
Mobile Platform 2 Pose: [-2.35656507703414;4.79561909221529;-9.52815295128575]
Robot Arm 2 end: [4.96046574902484;-9.53518771727492;-0.0433097676189692]
Robot Arm 1 togoal: 15.049207651401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.687872e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.712123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.781683292515784;-5.33590308547743;10.9161447246717]
Robot Arm 1 end: [-5.53023371101566;10.914369072566;1.48748252611706]
Robot Arm 1 togoal: 15.5571640519951
Mobile Platform 2 Pose: [-2.35656382429454;4.76333441839275;-9.93469428382739]
Robot Arm 2 end: [4.0657878873168;-9.93588832849812;0.334311619430407]
Robot Arm 1 togoal: 14.1159033897937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.686308e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.715361e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.781683900511742;-5.21538495674352;11.0395005668803]
Robot Arm 1 end: [-5.84141539883913;11.0403097560716;1.19760481250408]
Robot Arm 1 togoal: 15.878830897596
Mobile Platform 2 Pose: [-2.35775918355978;4.72513323787062;-10.2060106981828]
Robot Arm 2 end: [4.14848612258357;-10.1948879163468;1.27251171210136]
Robot Arm 1 togoal: 14.1520100088327

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.691355e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.705581e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.781681688865057;-5.09532296922401;10.9499575641824]
Robot Arm 1 end: [-5.88110514285016;10.9535659628682;0.683804496815094]
Robot Arm 1 togoal: 15.9317194588141
Mobile Platform 2 Pose: [-2.35775551362725;4.69124266842621;-9.63669630856248]
Robot Arm 2 end: [4.85985840911332;-9.64343409729664;1.492467153221]
Robot Arm 1 togoal: 14.8641631215892

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.701637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.702349e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.781673876042984;-4.97752352546755;10.9257219151764]
Robot Arm 1 end: [-5.49808237909431;10.9316453627313;0.123883615068686]
Robot Arm 1 togoal: 15.5887925993047
Mobile Platform 2 Pose: [-2.35689115990239;4.5817182763632;-9.49092889695568]
Robot Arm 2 end: [5.28978526257204;-9.51050829154823;1.05975601641755]
Robot Arm 1 togoal: 15.3045706088279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.718643e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.709212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.781670598105701;-4.94375118151579;10.629798022215]
Robot Arm 1 end: [-4.68166807097491;10.62411963984;-0.0392655631022139]
Robot Arm 1 togoal: 14.7775279156739
Mobile Platform 2 Pose: [-2.3546715661147;4.4673605537435;-9.47465400583325]
Robot Arm 2 end: [5.14810656879927;-9.48828299129785;0.324978630333127]
Robot Arm 1 togoal: 15.2038631286063

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.734155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.725416e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.781672991812396;-4.91141926731857;10.2371476094596]
Robot Arm 1 end: [-4.10044014628122;10.2246981878766;0.741308104193783]
Robot Arm 1 togoal: 14.1237679479266
Mobile Platform 2 Pose: [-2.35334610210828;4.39669980051282;-9.68695816322763]
Robot Arm 2 end: [4.25621512036153;-9.68683449747701;-0.0678625934012574]
Robot Arm 1 togoal: 14.3478997254199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.730161e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.743225e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.781676054538243;-4.87819666999686;10.607243901845]
Robot Arm 1 end: [-4.54888154099684;10.6048853250688;1.45403758283249]
Robot Arm 1 togoal: 14.5616044554079
Mobile Platform 2 Pose: [-2.35334839632442;4.36570722105068;-10.1173722037078]
Robot Arm 2 end: [3.55765763523501;-10.1063341849255;0.786547320092189]
Robot Arm 1 togoal: 13.5779530214298

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.722553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.735625e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.781676187983104;-4.84359120103929;10.9107604297602]
Robot Arm 1 end: [-5.21891307542532;10.9136203676108;1.41943236129605]
Robot Arm 1 togoal: 15.2466499060842
Mobile Platform 2 Pose: [-2.35335069210233;4.33326746533185;-9.74136485165766]
Robot Arm 2 end: [4.14247938770112;-9.73814331134071;1.4997529451716]
Robot Arm 1 togoal: 14.1449193562313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.721379e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.729797e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.781677162635759;-4.72632468756203;11.0194928275803]
Robot Arm 1 end: [-5.48946089921701;11.0250732315675;0.912017349762112]
Robot Arm 1 togoal: 15.5352835167801
Mobile Platform 2 Pose: [-2.35151786094168;4.29009012816657;-9.47543279487537]
Robot Arm 2 end: [4.87515106893939;-9.48778122979343;1.25309420402423]
Robot Arm 1 togoal: 14.8863783677508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.728790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.732187e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.78167053577739;-4.61094966882507;10.9077291481983]
Robot Arm 1 end: [-5.22356006959077;10.9172968787674;0.225576889575242]
Robot Arm 1 togoal: 15.3060881936325
Mobile Platform 2 Pose: [-2.34763657352528;4.17767939210222;-9.37516348847875]
Robot Arm 2 end: [4.92338552857007;-9.38721275891675;0.463408484909752]
Robot Arm 1 togoal: 14.973357794762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.743477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.746328e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.781663643769201;-4.53147088733158;10.7109395329027]
Robot Arm 1 end: [-4.31252567010888;10.7113918099384;-0.0464607800083637]
Robot Arm 1 togoal: 14.4156651827669
Mobile Platform 2 Pose: [-2.34561973417876;4.11913010230499;-9.60851371789957]
Robot Arm 2 end: [4.0678308875991;-9.60533373135117;-0.0773735265740733]
Robot Arm 1 togoal: 14.1638421791599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.758751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.762626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.78166613220369;-4.5001422749765;10.2623851463839]
Robot Arm 1 end: [-3.70396639076085;10.2556141928083;0.824558357539877]
Robot Arm 1 togoal: 13.7240323672025
Mobile Platform 2 Pose: [-2.34562179932535;4.08872970838966;-10.0490416948512]
Robot Arm 2 end: [3.28205184616439;-10.0351583152317;0.773132588207853]
Robot Arm 1 togoal: 13.3031365856684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.751564e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.753637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.781668436734235;-4.46750297361378;10.6624688586998]
Robot Arm 1 end: [-4.2918930747984;10.6640215927894;1.49169383136499]
Robot Arm 1 togoal: 14.3073404652535
Mobile Platform 2 Pose: [-2.34562427220171;4.05680422245464;-9.70104587102202]
Robot Arm 2 end: [3.91783602296587;-9.6965863448341;1.50526459822386]
Robot Arm 1 togoal: 13.9211517846469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.744977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.747891e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.781668730435203;-4.4273561902806;10.9347813194925]
Robot Arm 1 end: [-4.9883823546215;10.9416631194843;1.25787387876685]
Robot Arm 1 togoal: 15.0202386929969
Mobile Platform 2 Pose: [-2.34366197295676;4.01519408488775;-9.43388805448479]
Robot Arm 2 end: [4.67177821704264;-9.44450781993289;1.14765866155601]
Robot Arm 1 togoal: 14.6870361668784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.746076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.751713e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.781665596850069;-4.31360471831664;11.0007874247703]
Robot Arm 1 end: [-5.06175684532051;11.0122843710234;0.536513849619371]
Robot Arm 1 togoal: 15.1278039349956
Mobile Platform 2 Pose: [-2.3400050728933;3.90557946645549;-9.37552209802556]
Robot Arm 2 end: [4.53710715596323;-9.38080733365416;0.260450092353829]
Robot Arm 1 togoal: 14.6047892861102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.757540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.767992e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.781659425131289;-4.2308412545646;10.8105642627956]
Robot Arm 1 end: [-4.32972534205747;10.8199559108083;-0.04909556093658]
Robot Arm 1 togoal: 14.4387865191278
Mobile Platform 2 Pose: [-2.34000233480525;3.87461539565214;-9.68686341976263]
Robot Arm 2 end: [3.45437237949873;-9.6771304130229;0.0475047191229656]
Robot Arm 1 togoal: 13.5386694043093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.773620e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.776905e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.781659482475953;-4.19991718342676;10.4133966387707]
Robot Arm 1 end: [-3.43303363677274;10.4083629087052;0.558789479373368]
Robot Arm 1 togoal: 13.4736410089433
Mobile Platform 2 Pose: [-2.34000823695983;3.84459036403737;-10.1611672081096]
Robot Arm 2 end: [3.19757963009937;-10.142725560525;1.17779225518571]
Robot Arm 1 togoal: 13.2028129667333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.770811e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.764387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.781682331201534;-4.09448015609572;10.4382872899844]
Robot Arm 1 end: [-3.79155706786077;10.4414325060787;1.4298041681589]
Robot Arm 1 togoal: 13.7989211786005
Mobile Platform 2 Pose: [-2.34000502641557;3.81250944101206;-9.6035358698931]
Robot Arm 2 end: [4.08235439084003;-9.60269557605662;1.44725350464927]
Robot Arm 1 togoal: 14.0881508559524

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.765333e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.763344e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.781680054751613;-4.06174592363373;10.847662947386]
Robot Arm 1 end: [-4.60835141829477;10.8589772654396;1.2385193725127]
Robot Arm 1 togoal: 14.6363099313947
Mobile Platform 2 Pose: [-2.33931680321408;3.73902267855255;-9.39696346187248]
Robot Arm 2 end: [4.5019741146594;-9.4057313474558;0.688111005532764]
Robot Arm 1 togoal: 14.5380229925101

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.765321e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.773563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.781684821659119;-3.95149974743486;10.9849526060845]
Robot Arm 1 end: [-4.63221994577172;11.0002516024945;0.417444869631409]
Robot Arm 1 togoal: 14.7078277707993
Mobile Platform 2 Pose: [-2.33994257896891;3.64918765595417;-9.56986956726233]
Robot Arm 2 end: [3.71905338338727;-9.56205937505173;-0.0455786472262477]
Robot Arm 1 togoal: 13.8151506102957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.777794e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789780e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.781875577205923;-3.91935982829879;10.7044829942263]
Robot Arm 1 end: [-3.72777758341135;10.7122327378456;-0.00699988411251268]
Robot Arm 1 togoal: 13.8309051221646
Mobile Platform 2 Pose: [-2.3399442564744;3.61982684495123;-10.0092472373757]
Robot Arm 2 end: [2.84639486801465;-9.99030787224595;0.77946317413429]
Robot Arm 1 togoal: 12.8677827113457

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789404e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.779827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.781878861705815;-3.88922940061953;10.2640505026052]
Robot Arm 1 end: [-3.16941961952566;10.2649960361701;0.941447818045947]
Robot Arm 1 togoal: 13.184829037418
Mobile Platform 2 Pose: [-2.33994647545298;3.58874407930127;-9.67842395427274]
Robot Arm 2 end: [3.56565234505974;-9.67268265622385;1.47893381168009]
Robot Arm 1 togoal: 13.5696657937755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.780893e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.776008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.781879205737492;-3.85755136663854;10.7073575131409]
Robot Arm 1 end: [-3.92952755900528;10.7153726690903;1.45038325591869]
Robot Arm 1 togoal: 13.9480637651723
Mobile Platform 2 Pose: [-2.34059492975937;3.5410612832715;-9.42570619311616]
Robot Arm 2 end: [4.25952226288926;-9.43228740661222;0.903592297127283]
Robot Arm 1 togoal: 14.2841683351217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.776989e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.783244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.78281551706289;-3.7982249706003;10.9402478697271]
Robot Arm 1 end: [-4.49683807664339;10.9535251599643;0.876413947054066]
Robot Arm 1 togoal: 14.5424556258823
Mobile Platform 2 Pose: [-2.34189309200398;3.43716489708831;-9.48532445619216]
Robot Arm 2 end: [3.68368165248437;-9.47802191156827;0.0203627560268575]
Robot Arm 1 togoal: 13.7756131199065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.783177e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785120601107628;-3.69177727337694;10.8550945392078]
Robot Arm 1 end: [-3.99043963798221;10.8706281631517;0.0716757039351418]
Robot Arm 1 togoal: 14.092229616979
Mobile Platform 2 Pose: [-2.34189356702888;3.40815086712256;-9.91530509972639]
Robot Arm 2 end: [2.66298833176714;-9.89554989847062;0.635040119905498]
Robot Arm 1 togoal: 12.694373092962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798240e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.791854e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.78511996853309;-3.66186862675056;10.4820926867959]
Robot Arm 1 end: [-2.98338029750333;10.4836611310313;0.475466664647142]
Robot Arm 1 togoal: 13.0343865216052
Mobile Platform 2 Pose: [-2.34256362853351;3.31220199399333;-9.75657930801079]
Robot Arm 2 end: [3.22704883007142;-9.74860519054193;1.44758721519839]
Robot Arm 1 togoal: 13.2296413231912

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796167e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789179e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.786664819823492;-3.58081519326214;10.2955047017751]
Robot Arm 1 end: [-3.32978936025929;10.3013094940598;1.37927905867103]
Robot Arm 1 togoal: 13.3339475259693
Mobile Platform 2 Pose: [-2.34256164156611;3.28110380983473;-9.44564059049978]
Robot Arm 2 end: [3.97893356434561;-9.44955761863045;0.862221700980364]
Robot Arm 1 togoal: 14.0052689820244

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.791212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796085e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.786662370339711;-3.54928568403919;10.7678157809355]
Robot Arm 1 end: [-4.14028375605411;10.7829024335484;1.06062831919696]
Robot Arm 1 togoal: 14.1694214069125
Mobile Platform 2 Pose: [-2.34388163660301;3.18005037782373;-9.56632925210102]
Robot Arm 2 end: [3.28782643231409;-9.55524243945683;0.0153782255246818]
Robot Arm 1 togoal: 13.3802480362189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.793770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808814e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.790330081140415;-3.44457839070916;10.8012765322033]
Robot Arm 1 end: [-3.8506203497948;10.8190792062796;0.180893237802405]
Robot Arm 1 togoal: 13.9393852430585
Mobile Platform 2 Pose: [-2.34388362827189;3.15167042995138;-10.0086191352673]
Robot Arm 2 end: [2.46258645277185;-9.9870049599368;0.895993550536348]
Robot Arm 1 togoal: 12.4782556082872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804707e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.794659e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.790329059890371;-3.41504832760259;10.4637880119308]
Robot Arm 1 end: [-2.78087274309455;10.4673180693227;0.487324378359076]
Robot Arm 1 togoal: 12.8311176264528
Mobile Platform 2 Pose: [-2.34388518866748;3.12071443476594;-9.64167301573287]
Robot Arm 2 end: [3.20805330691872;-9.63600161641308;1.4196272132933]
Robot Arm 1 togoal: 13.2134569066367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799743e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.792354625246908;-3.36431606736125;10.2203484373091]
Robot Arm 1 end: [-3.08491434850033;10.2263870763064;1.33837754231151]
Robot Arm 1 togoal: 13.0881467587264
Mobile Platform 2 Pose: [-2.34455274726061;3.02719460759773;-9.47805678256634]
Robot Arm 2 end: [3.70399456599006;-9.4810502996256;0.617979674294711]
Robot Arm 1 togoal: 13.7435156188233

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800134e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.79235116415793;-3.33337103931772;10.7048379018684]
Robot Arm 1 end: [-3.9465606649325;10.7217800045088;0.966115685303454]
Robot Arm 1 togoal: 13.976244751658
Mobile Platform 2 Pose: [-2.34521726043233;2.9500429382923;-9.66882913104008]
Robot Arm 2 end: [2.62591700472423;-9.65196083042248;0.112015626264672]
Robot Arm 1 togoal: 12.7090577321521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.795595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.796439161190767;-3.22656698446887;10.7878434863506]
Robot Arm 1 end: [-3.63841074465375;10.8079194190667;0.204125073371835]
Robot Arm 1 togoal: 13.7256380908129
Mobile Platform 2 Pose: [-2.34522266400889;2.92139081888681;-10.1487235906087]
Robot Arm 2 end: [2.63734728458392;-10.1349751141434;1.34753437126359]
Robot Arm 1 togoal: 12.6392584828767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805571e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800555e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.796438221358596;-3.19744269612249;10.4528775196229]
Robot Arm 1 end: [-2.56138115607363;10.4566726523591;0.843351105218424]
Robot Arm 1 togoal: 12.5879328916297
Mobile Platform 2 Pose: [-2.34521932104811;2.89065990427595;-9.61250545622799]
Robot Arm 2 end: [3.5264385874834;-9.61324531103915;0.963445962454481]
Robot Arm 1 togoal: 13.5434481043483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.796439135698003;-3.14639919082386;10.6992428490632]
Robot Arm 1 end: [-3.17520178355792;10.7067730606218;1.38169639476802]
Robot Arm 1 togoal: 13.1948806622538
Mobile Platform 2 Pose: [-2.34808310282791;2.78923105695881;-9.65868655873299]
Robot Arm 2 end: [2.86937730213055;-9.64630799594692;0.0440358383134845]
Robot Arm 1 togoal: 12.9586802303796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808656e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.796422420873573;-3.08947767297862;10.9310706970581]
Robot Arm 1 end: [-3.63023290954174;10.9477204861924;0.36090020674453]
Robot Arm 1 togoal: 13.7123030457547
Mobile Platform 2 Pose: [-2.34808556307301;2.7612222953832;-10.1045926332993]
Robot Arm 2 end: [2.42381625347382;-10.088270457013;1.31095420109722]
Robot Arm 1 togoal: 12.425905247156

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801272e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801308e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.796420917749269;-3.05927938423274;10.5546177545205]
Robot Arm 1 end: [-2.43148552526131;10.5589621402736;0.498676651171417]
Robot Arm 1 togoal: 12.4859687677249
Mobile Platform 2 Pose: [-2.34808387107366;2.73086084885946;-9.49950197117755]
Robot Arm 2 end: [3.40503808916311;-9.496433759128;0.779204590059593]
Robot Arm 1 togoal: 13.434987367358

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802987e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.79641882941476;-2.99682763270401;10.3354132478148]
Robot Arm 1 end: [-3.38935927626555;10.3446278771596;1.23439383700739]
Robot Arm 1 togoal: 13.3968598296503
Mobile Platform 2 Pose: [-2.34876663304519;2.67884826983158;-9.7360597129151]
Robot Arm 2 end: [2.15690822488654;-9.71688134799322;0.290196850228147]
Robot Arm 1 togoal: 12.2223340714005

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.796415721687857;-2.96554376350193;10.7993593083586]
Robot Arm 1 end: [-3.26944771469665;10.8148638267787;0.142342914577187]
Robot Arm 1 togoal: 13.3657375804124
Mobile Platform 2 Pose: [-2.34877368070339;2.6494293522461;-10.1628803209163]
Robot Arm 2 end: [2.80421604291347;-10.1611290897248;1.3786961647918]
Robot Arm 1 togoal: 12.8060205153152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806876e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803252e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.796417782459676;-2.93712506782866;10.2255630574632]
Robot Arm 1 end: [-2.67455331354572;10.2336444359276;1.30423713922415]
Robot Arm 1 togoal: 12.6785597588277
Mobile Platform 2 Pose: [-2.34877020400985;2.61978897559962;-9.63718423960662]
Robot Arm 2 end: [2.87919986838742;-9.63143020841983;0.099576935374586]
Robot Arm 1 togoal: 12.9626415699033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807535e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.796415844191542;-2.9057986582671;10.8188524552602]
Robot Arm 1 end: [-3.54664025657797;10.8375575124389;0.627144373100621]
Robot Arm 1 togoal: 13.6019095243838
Mobile Platform 2 Pose: [-2.34877356693635;2.59164736107897;-10.2639686919111]
Robot Arm 2 end: [2.29808800015876;-10.2497662537469;1.32823751212649]
Robot Arm 1 togoal: 12.3021343277454

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806159e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803667e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.79641547004999;-2.87720942993043;10.4522436395456]
Robot Arm 1 end: [-2.27091121753404;10.4575447172608;0.910709835750094]
Robot Arm 1 togoal: 12.2945948839918
Mobile Platform 2 Pose: [-2.3487703990534;2.56220617809858;-9.68643802007618]
Robot Arm 2 end: [3.07100815780206;-9.68567539234912;0.284019162480062]
Robot Arm 1 togoal: 13.1331708209668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797108e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807639e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.796416236091228;-2.8457272064553;10.8133594586919]
Robot Arm 1 end: [-3.37832001356379;10.8296248105794;1.08755209832888]
Robot Arm 1 togoal: 13.41102534466
Mobile Platform 2 Pose: [-2.3487726718193;2.53417455900763;-10.2263419451476]
Robot Arm 2 end: [2.08279727410883;-10.207793092865;1.21969011688296]
Robot Arm 1 togoal: 12.0883396536629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805163e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803761e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.796388827514705;-2.74951375939864;10.9347667819554]
Robot Arm 1 end: [-2.16319334190781;10.9399490103257;0.481464016345222]
Robot Arm 1 togoal: 12.2436682197761
Mobile Platform 2 Pose: [-2.3487698097988;2.50482119045923;-9.67640648266725]
Robot Arm 2 end: [3.05859670357302;-9.67480824519867;0.346967877511077]
Robot Arm 1 togoal: 13.1152982798179

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807609e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.796392100718804;-2.71855950859352;10.4668511051392]
Robot Arm 1 end: [-3.08756801952176;10.4776464507676;1.25137261820847]
Robot Arm 1 togoal: 13.0990565203205
Mobile Platform 2 Pose: [-2.34877249558102;2.47685063398366;-10.2003846669422]
Robot Arm 2 end: [2.04707447386645;-10.1823322305558;1.23559668937803]
Robot Arm 1 togoal: 12.0518340293458

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806465e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.796389840809027;-2.69021949712447;10.9627793031222]
Robot Arm 1 end: [-2.11559784888716;10.9684082867639;0.459269957901906]
Robot Arm 1 togoal: 12.2005244366021
Mobile Platform 2 Pose: [-2.3487695295908;2.44788322036504;-9.64965835377187]
Robot Arm 2 end: [2.86216313891144;-9.64495457488494;0.199511369236815]
Robot Arm 1 togoal: 12.9347450715932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798213e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.796391770714565;-2.65919581614001;10.4062783318304]
Robot Arm 1 end: [-3.06390375049151;10.4180805223941;1.22158939376958]
Robot Arm 1 togoal: 13.0740207754323
Mobile Platform 2 Pose: [-2.34877418566269;2.4196954570809;-10.2918383069717]
Robot Arm 2 end: [2.19725222730233;-10.2798969059621;1.35065257524786]
Robot Arm 1 togoal: 12.201652446268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807222e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806252e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.796389816125231;-2.63118986201349;10.9377083323144]
Robot Arm 1 end: [-2.00582010564047;10.9428429472628;0.652723666676769]
Robot Arm 1 togoal: 12.0740454290686
Mobile Platform 2 Pose: [-2.34877075931392;2.39131524505549;-9.72308318737441]
Robot Arm 2 end: [2.3827535546714;-9.71297284264395;0.0571570862603726]
Robot Arm 1 togoal: 12.4722815178656

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798670e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803988e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.796390004035184;-2.60036337341877;10.4970759978409]
Robot Arm 1 end: [-3.19548095511403;10.5153123996472;0.955915423439594]
Robot Arm 1 togoal: 13.2176240332623
Mobile Platform 2 Pose: [-2.34877528755224;2.36220333059;-10.4070271006548]
Robot Arm 2 end: [2.45958311425031;-10.4059973495857;1.37838230635239]
Robot Arm 1 togoal: 12.4670118727514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808418e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807152e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.796390072302378;-2.57284959128125;10.9047885588807]
Robot Arm 1 end: [-2.09085036120836;10.9117390334426;1.12391624113445]
Robot Arm 1 togoal: 12.131677791377
Mobile Platform 2 Pose: [-2.34877165529481;2.33419721848822;-9.82881477216693]
Robot Arm 2 end: [1.88280098864288;-9.81001403839509;0.237491653654144]
Robot Arm 1 togoal: 11.9534282813107

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801195e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802172e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.796363225241669;-2.47324192579118;10.8008331327439]
Robot Arm 1 end: [-2.88370054969864;10.8207483493078;0.230072562569623]
Robot Arm 1 togoal: 12.9741998348995
Mobile Platform 2 Pose: [-2.34877318182378;2.30449234125399;-10.3633539922705]
Robot Arm 2 end: [2.77904313043414;-10.3689854093385;1.18114984691767]
Robot Arm 1 togoal: 12.7888854748313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806976e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.796368542544768;-2.44526519424904;10.2770413335241]
Robot Arm 1 end: [-2.64091557246017;10.2840401067885;1.32067876539017]
Robot Arm 1 togoal: 12.6456930964406
Mobile Platform 2 Pose: [-2.34877072876919;2.2771191766684;-9.79720153282717]
Robot Arm 2 end: [1.62766385472848;-9.77446009866569;0.832044226015615]
Robot Arm 1 togoal: 11.6502401273175

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804240e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.796365272930894;-2.41652443235259;10.7815330061855]
Robot Arm 1 end: [-1.87637209333515;10.7877776896832;0.364499793873653]
Robot Arm 1 togoal: 11.9585241779445
Mobile Platform 2 Pose: [-2.34674051271469;2.23631747494243;-10.0540980314664]
Robot Arm 2 end: [2.78108519913866;-10.049002105919;0.350878492367877]
Robot Arm 1 togoal: 12.8346298901312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807583e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.7963648672244;-2.387334957106;10.2841614853545]
Robot Arm 1 end: [-3.01509094788663;10.3022959941094;0.653053511623272]
Robot Arm 1 togoal: 13.047501920157
Mobile Platform 2 Pose: [-2.34674582680323;2.20857640531537;-10.3346455122174]
Robot Arm 2 end: [2.16903806563783;-10.3276257817191;1.37677245521872]
Robot Arm 1 togoal: 12.174301938118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804129e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808372e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.796366568866998;-2.35859860187651;10.5902533027717]
Robot Arm 1 end: [-1.9478559001864;10.5971112607795;1.21684738120173]
Robot Arm 1 togoal: 11.9666332476971
Mobile Platform 2 Pose: [-2.34674234712692;2.18118389176474;-9.82910831230186]
Robot Arm 2 end: [1.62626033399584;-9.80822817736525;0.363775896705061]
Robot Arm 1 togoal: 11.6852844813915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.796360644668889;-2.28803035807725;10.7865453309741]
Robot Arm 1 end: [-1.68007243583752;10.7917748974274;0.589394425252597]
Robot Arm 1 togoal: 11.7438872840941
Mobile Platform 2 Pose: [-2.34674137279777;2.15220886277138;-10.2331444522491]
Robot Arm 2 end: [2.80654883540132;-10.2321448674883;0.712267401387976]
Robot Arm 1 togoal: 12.834158824367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808121e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.796359200315792;-2.25931558820479;10.501140275187]
Robot Arm 1 end: [-2.76123257128072;10.5246410834271;0.335368825659917]
Robot Arm 1 togoal: 12.8269258314805
Mobile Platform 2 Pose: [-2.34674414219582;2.12478110384644;-9.89536539728089]
Robot Arm 2 end: [1.85756203394604;-9.88102379402802;1.32142494467869]
Robot Arm 1 togoal: 11.8598382336848

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805988e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805681e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.796365598690235;-2.23111347770386;10.1168653053973]
Robot Arm 1 end: [-2.40930464075901;10.1234058883833;1.32013539302289]
Robot Arm 1 togoal: 12.4115437102234
Mobile Platform 2 Pose: [-2.34577599180185;2.06492261230638;-9.68038435796778]
Robot Arm 2 end: [1.43732264844121;-9.6584062492529;0.544072157261718]
Robot Arm 1 togoal: 11.4840508386637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805627e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806456e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.796361838676235;-2.20299449039015;10.6338445451522]
Robot Arm 1 end: [-1.56667259290742;10.639081004841;0.637004601539771]
Robot Arm 1 togoal: 11.6179941154557
Mobile Platform 2 Pose: [-2.3451727966739;2.02381573001856;-9.93402371277648]
Robot Arm 2 end: [2.50169945619641;-9.92434744842351;0.276963897001338]
Robot Arm 1 togoal: 12.563671375326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806182e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.796359211337681;-2.1751374338238;10.3299339484801]
Robot Arm 1 end: [-2.36149487256356;10.3479576906897;0.126326159286828]
Robot Arm 1 togoal: 12.4447878712694
Mobile Platform 2 Pose: [-2.34517969845452;1.99569494202493;-10.3191656139316]
Robot Arm 2 end: [2.14205412850444;-10.3177701657643;1.35452941899837]
Robot Arm 1 togoal: 12.1473523367078

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.809746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.796363257832599;-2.14548021699666;9.98926512936768]
Robot Arm 1 end: [-2.39329719048363;10.0000264614781;1.3018862128051]
Robot Arm 1 togoal: 12.3952340076373
Mobile Platform 2 Pose: [-2.34517548836307;1.96894583878359;-9.78175784871893]
Robot Arm 2 end: [1.3269346508525;-9.75944627707163;0.84540460856099]
Robot Arm 1 togoal: 11.349614258257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811527e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.796361679805544;-2.11942477504853;10.5688821918715]
Robot Arm 1 end: [-1.76159469146173;10.5772206044501;1.22485450199799]
Robot Arm 1 togoal: 11.7794734801013
Mobile Platform 2 Pose: [-2.34579492561959;1.90079865707208;-9.98160567656085]
Robot Arm 2 end: [1.37426664252056;-9.9615296865384;0.343175981201531]
Robot Arm 1 togoal: 11.4351515529718

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.788458e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.797685679089568;-2.02677610071063;10.4182876195168]
Robot Arm 1 end: [-1.44823617263398;10.4242625778502;0.989260426730546]
Robot Arm 1 togoal: 11.4684287142202
Mobile Platform 2 Pose: [-2.3468124652648;1.83915275700458;-9.76876881065231]
Robot Arm 2 end: [2.07477909643231;-9.75267507125388;0.11651978507881]
Robot Arm 1 togoal: 12.1587015968862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804686e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804859e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.797687169479083;-2.00168559156978;10.7107340393054]
Robot Arm 1 end: [-1.39781758527843;10.7166582773944;0.837423799103519]
Robot Arm 1 togoal: 11.4407657617955
Mobile Platform 2 Pose: [-2.3468157265013;1.81060449597734;-10.2707538299005]
Robot Arm 2 end: [2.20351819348693;-10.2632231746101;1.23347341040431]
Robot Arm 1 togoal: 12.2097425967193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810969e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.797687594738336;-1.97447621415032;10.4086272265384]
Robot Arm 1 end: [-1.81022995947623;10.4180213531694;0.110569362345159]
Robot Arm 1 togoal: 11.9014952056895
Mobile Platform 2 Pose: [-2.34681599955302;1.78437832688399;-9.63731912904549]
Robot Arm 2 end: [1.39012521492265;-9.61912299981582;1.23664670722938]
Robot Arm 1 togoal: 11.400038442742

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.787503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799800e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.800185646140179;-1.88005352218567;10.3010765769743]
Robot Arm 1 end: [-2.04656100395039;10.3165701808437;0.126299087037376]
Robot Arm 1 togoal: 12.1311598513051
Mobile Platform 2 Pose: [-2.35040955236264;1.69840952948481;-9.77950762953162]
Robot Arm 2 end: [1.18883105431277;-9.75941039691613;1.1144545562806]
Robot Arm 1 togoal: 11.1987991729861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.790917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.803088110776733;-1.7839020148824;10.1718711352958]
Robot Arm 1 end: [-2.40007464381347;10.1972458988548;0.633730950877272]
Robot Arm 1 togoal: 12.4333424099191
Mobile Platform 2 Pose: [-2.35238175079688;1.66674199620317;-9.51687146853982]
Robot Arm 2 end: [1.05919264374358;-9.49451425967082;0.94537948130193]
Robot Arm 1 togoal: 11.0856933363076

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.788512e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807587e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.80621440897781;-1.69170389783706;10.127055719979]
Robot Arm 1 end: [-2.30864301859343;10.135157609547;0.764612031210507]
Robot Arm 1 togoal: 12.3326024543856
Mobile Platform 2 Pose: [-2.35238672131385;1.64171785333686;-9.80762160853295]
Robot Arm 2 end: [1.01178802171447;-9.78518121260982;0.600736284980868]
Robot Arm 1 togoal: 11.0522625670842

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803318e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798974e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.80621054218357;-1.6628170673463;10.4912039901931]
Robot Arm 1 end: [-1.35655553098941;10.4999170094989;1.26970500566564]
Robot Arm 1 togoal: 11.3703306776457
Mobile Platform 2 Pose: [-2.35445257087807;1.56994380255977;-9.61751154613723]
Robot Arm 2 end: [1.00558844437056;-9.59657064198341;0.407448174172286]
Robot Arm 1 togoal: 11.0691341089796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.813130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.806473713057909;-1.63752467183121;10.7675385987738]
Robot Arm 1 end: [-1.23763293234308;10.7757951155355;1.18396593351077]
Robot Arm 1 togoal: 11.2694207457768
Mobile Platform 2 Pose: [-2.3544509605387;1.54450280449632;-9.93249491989555]
Robot Arm 2 end: [1.47214093453275;-9.91668217152969;0.0859047143295376]
Robot Arm 1 togoal: 11.5618535694445

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.788742e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.808405947968007;-1.57880856845023;10.5517713792692]
Robot Arm 1 end: [-1.02664238360297;10.5583650074564;1.02125384714856]
Robot Arm 1 togoal: 11.052074916264
Mobile Platform 2 Pose: [-2.3582729439747;1.45189334356407;-10.0349904418268]
Robot Arm 2 end: [1.71927752365967;-10.0200244624888;0.138999447789112]
Robot Arm 1 togoal: 11.8004996581485

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810657e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.790045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.810425594004292;-1.54134256945398;10.8022072717571]
Robot Arm 1 end: [-0.970906958374863;10.8085964024109;0.934829014009211]
Robot Arm 1 togoal: 11.016270877939
Mobile Platform 2 Pose: [-2.36186332634858;1.36067763640665;-10.1345897309754]
Robot Arm 2 end: [1.81698416532577;-10.1192639836953;0.276465615268732]
Robot Arm 1 togoal: 11.8829375364909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.810427446920923;-1.51527839465801;10.4661046823648]
Robot Arm 1 end: [-1.00392154376038;10.473037654909;0.35904165269336]
Robot Arm 1 togoal: 11.0752065969476
Mobile Platform 2 Pose: [-2.3651514090472;1.26855965407989;-9.99693852207886]
Robot Arm 2 end: [1.87814984610237;-9.9837414092953;0.535992972201898]
Robot Arm 1 togoal: 11.9189322908155

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803014e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789968e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.814090076051827;-1.42920267159886;10.6218872248039]
Robot Arm 1 end: [-1.0198531236006;10.6299566002118;0.271012116862669]
Robot Arm 1 togoal: 11.1083967291356
Mobile Platform 2 Pose: [-2.36807037591931;1.17765357872688;-10.0712827178803]
Robot Arm 2 end: [1.81282232361085;-10.0666591194889;0.720168387704795]
Robot Arm 1 togoal: 11.8401244063845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810373e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807830e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.814088505865445;-1.40301006454507;10.2983067203764]
Robot Arm 1 end: [-1.5535577564105;10.3168523483558;0.120434930565549]
Robot Arm 1 togoal: 11.6424514066021
Mobile Platform 2 Pose: [-2.36807085424371;1.1503325565085;-9.76296198267911]
Robot Arm 2 end: [1.15230830074447;-9.75044333807456;1.35827165494875]
Robot Arm 1 togoal: 11.1562869932899

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.793675e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.817233503003674;-1.312388970875;10.2115377084592]
Robot Arm 1 end: [-1.59229633688824;10.2325388049732;0.179408819732893]
Robot Arm 1 togoal: 11.6719867868766
Mobile Platform 2 Pose: [-2.36916802265985;1.0887152969348;-9.55813051898271]
Robot Arm 2 end: [0.965774014337785;-9.54493624247021;1.34201133103036]
Robot Arm 1 togoal: 10.976671613042

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.792283e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804504e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.81991280441761;-1.22096870954646;10.3120363719602]
Robot Arm 1 end: [-1.67712693904355;10.3405590643917;0.309429667966466]
Robot Arm 1 togoal: 11.7447510274309
Mobile Platform 2 Pose: [-2.37082272678237;1.00430287251271;-9.69512814828859]
Robot Arm 2 end: [0.722726640805718;-9.68015376144073;1.29163884632678]
Robot Arm 1 togoal: 10.7299475659753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.819914655975346;-1.13863112397155;10.1388029483211]
Robot Arm 1 end: [-1.66653278495222;10.1664651687366;0.417594196298077]
Robot Arm 1 togoal: 11.7197782505648
Mobile Platform 2 Pose: [-2.37158708128355;0.924124826228933;-9.52561088436811]
Robot Arm 2 end: [0.562287429389977;-9.50838907022407;1.23761057960384]
Robot Arm 1 togoal: 10.5775189251032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806968e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.819914883304494;-1.11116671868577;10.4428369463863]
Robot Arm 1 end: [-1.50600348942758;10.4669641228218;1.18994230990337]
Robot Arm 1 togoal: 11.5202331133052
Mobile Platform 2 Pose: [-2.37158886212251;0.899811458832491;-9.82172336965654]
Robot Arm 2 end: [0.388646700044734;-9.80097457615309;1.09807877384883]
Robot Arm 1 togoal: 10.3991563956514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.819936490033113;-1.02845374510296;10.5282817691811]
Robot Arm 1 end: [-1.34329384119413;10.536989951539;1.24016486215879]
Robot Arm 1 togoal: 11.3594692547836
Mobile Platform 2 Pose: [-2.37163720658625;0.855680118889129;-9.58610276247598]
Robot Arm 2 end: [0.279356793918873;-9.56384924189384;0.971270235997312]
Robot Arm 1 togoal: 10.3032813411026

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800198e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816871e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.819932933546378;-1.00248648307461;10.8113202372108]
Robot Arm 1 end: [-1.09162189651295;10.8249270697581;1.32815826198571]
Robot Arm 1 togoal: 11.1239277641283
Mobile Platform 2 Pose: [-2.3716378070606;0.831760886508469;-9.91299876643516]
Robot Arm 2 end: [0.199009656735756;-9.88923947999496;0.759309649687202]
Robot Arm 1 togoal: 10.2280124198566

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.813936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818329e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.819939964803792;-0.977542576770136;10.4719454029542]
Robot Arm 1 end: [-0.831363309919303;10.4764593196377;1.30694104544523]
Robot Arm 1 togoal: 10.8439506578904
Mobile Platform 2 Pose: [-2.37115572663592;0.798458224780793;-9.6579028975047]
Robot Arm 2 end: [0.174218709811132;-9.63496500921976;0.671882529784748]
Robot Arm 1 togoal: 10.2161136141778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812730e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817180e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.819933527458232;-0.952280067848015;10.8054659655469]
Robot Arm 1 end: [-0.495746544999765;10.8127490517086;1.13424131157753]
Robot Arm 1 togoal: 10.5342697345852
Mobile Platform 2 Pose: [-2.37115567466202;0.774663938359655;-9.98544039943602]
Robot Arm 2 end: [0.186153088202399;-9.96324197982593;0.490068270896779]
Robot Arm 1 togoal: 10.2382560096148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817331e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818036e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.819938037170051;-0.928638452220949;10.4412553915887]
Robot Arm 1 end: [-0.511735012008895;10.4482285799615;1.15467871351941]
Robot Arm 1 togoal: 10.5276622812245
Mobile Platform 2 Pose: [-2.37017949849741;0.749220239729778;-9.71545379222732]
Robot Arm 2 end: [0.213752384098106;-9.69455170944235;0.400106081063596]
Robot Arm 1 togoal: 10.2796128136902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812412e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816258e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.819934711533187;-0.903767296652338;10.7563048965592]
Robot Arm 1 end: [-0.348661102136761;10.7628720537785;0.984970300524583]
Robot Arm 1 togoal: 10.3905768952467
Mobile Platform 2 Pose: [-2.37017828802476;0.725237758446195;-10.0404574047931]
Robot Arm 2 end: [0.316092576330314;-10.0211166654268;0.241949360890235]
Robot Arm 1 togoal: 10.3951037750656

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.819936557161101;-0.88029124258093;10.4049912790265]
Robot Arm 1 end: [-0.376170494005457;10.4121383705776;1.04590953183964]
Robot Arm 1 togoal: 10.3952144330989
Mobile Platform 2 Pose: [-2.36877168472926;0.696087833775069;-9.77617787351173]
Robot Arm 2 end: [0.421539156306602;-9.75707183927113;0.16226348511193]
Robot Arm 1 togoal: 10.5125476167577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.819935817273139;-0.855721702882876;10.6918389220225]
Robot Arm 1 end: [-0.260322815515976;10.6984859889501;0.867824964763735]
Robot Arm 1 togoal: 10.3047923114449
Mobile Platform 2 Pose: [-2.368769664346;0.671475489128891;-10.1024676034504]
Robot Arm 2 end: [0.730250728832519;-10.0819010005655;0.0975129401327693]
Robot Arm 1 togoal: 10.824569455046

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818120e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804756e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.819935316600273;-0.832389542110023;10.3679798996546]
Robot Arm 1 end: [-0.286581780688717;10.3748930257943;0.965561878384058]
Robot Arm 1 togoal: 10.3083859560326
Mobile Platform 2 Pose: [-2.36707439144722;0.594595246765641;-9.92337679818336]
Robot Arm 2 end: [0.743775422605251;-9.90074098795333;0.1183507733693]
Robot Arm 1 togoal: 10.8354043549753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815120e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.819938084663119;-0.793099797427753;10.6145791455381]
Robot Arm 1 end: [-0.190359789573238;10.6214493932373;0.824960171723434]
Robot Arm 1 togoal: 10.2329909328531
Mobile Platform 2 Pose: [-2.36522731948523;0.553557873135875;-10.1681698173803]
Robot Arm 2 end: [0.8857493507739;-10.1412019749357;0.194022284976815]
Robot Arm 1 togoal: 10.9672397065884

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806820e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.819936350177914;-0.769942418845798;10.3250464222665]
Robot Arm 1 end: [-0.222876590154458;10.3319002096752;0.959002366954653]
Robot Arm 1 togoal: 10.243690978659
Mobile Platform 2 Pose: [-2.36328815709657;0.512135779353672;-9.92391520645126]
Robot Arm 2 end: [0.967829712970224;-9.8973941636049;0.301889922766148]
Robot Arm 1 togoal: 11.0358527514093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815454e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802956e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.819941520716849;-0.717354010951284;10.546587900378]
Robot Arm 1 end: [-0.119949658363985;10.5536563873552;0.844268328159767]
Robot Arm 1 togoal: 10.1576514135282
Mobile Platform 2 Pose: [-2.36131783885167;0.444354461960484;-10.1210612221429]
Robot Arm 2 end: [1.01209505159371;-10.0971850335327;0.471395604991273]
Robot Arm 1 togoal: 11.0624297395328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819101e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.809299e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.819936261442244;-0.67978592777261;10.3007605876116]
Robot Arm 1 end: [-0.160321836645357;10.3078034984962;1.01160985605746]
Robot Arm 1 togoal: 10.1777385079714
Mobile Platform 2 Pose: [-2.36131735854982;0.419247780175444;-9.81662941583603]
Robot Arm 2 end: [1.03710378600711;-9.80124880478066;0.729847966367572]
Robot Arm 1 togoal: 11.0672075773889

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.819943265225182;-0.613538142225328;10.496632555605]
Robot Arm 1 end: [-0.0394383968628971;10.5038714077004;0.921887669854075]
Robot Arm 1 togoal: 10.0699128746994
Mobile Platform 2 Pose: [-2.35744447065762;0.333455286536869;-9.89610189391633]
Robot Arm 2 end: [0.91963004861956;-9.88899384656747;0.919297428618311]
Robot Arm 1 togoal: 10.9367586034082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810681e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.819936553540179;-0.545732839700643;10.3076333849628]
Robot Arm 1 end: [-0.0997228852165707;10.3151561415133;1.11339189181363]
Robot Arm 1 togoal: 10.1128566746276
Mobile Platform 2 Pose: [-2.35744355716756;0.308974974785555;-9.61224475767974]
Robot Arm 2 end: [0.835465762935029;-9.60978132953064;1.04106057610395]
Robot Arm 1 togoal: 10.853106963498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.819950442671284;-0.465579262895683;10.4454320423201]
Robot Arm 1 end: [0.0459938834782795;10.4525751213648;1.0462901441918]
Robot Arm 1 togoal: 9.97559078227609
Mobile Platform 2 Pose: [-2.35573558503084;0.251982147048147;-9.82800410634776]
Robot Arm 2 end: [0.648675680883618;-9.82621448375385;1.19494179798349]
Robot Arm 1 togoal: 10.6550837215576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820411e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816764e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.819936588420025;-0.390685756950886;10.374548443558]
Robot Arm 1 end: [-0.0876854908817972;10.3829687368662;1.23091075655085]
Robot Arm 1 togoal: 10.0991195351639
Mobile Platform 2 Pose: [-2.35421143051226;0.223650806450316;-9.5621614362968]
Robot Arm 2 end: [0.505788334082585;-9.56129574978223;1.26615780090214]
Robot Arm 1 togoal: 10.5180318732235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.813873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.819954452946025;-0.310075621146704;10.3362302651904]
Robot Arm 1 end: [0.099729363638168;10.3425724962189;1.16473838249215]
Robot Arm 1 togoal: 9.91259990761673
Mobile Platform 2 Pose: [-2.35421052310603;0.199303188524974;-9.84451507609727]
Robot Arm 2 end: [0.280617816788899;-9.83848809919591;1.33361454066002]
Robot Arm 1 togoal: 10.2835938318931

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819171e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.819942028651978;-0.248827726690595;10.5361225704788]
Robot Arm 1 end: [-0.1218978645792;10.5460212825966;1.29961984236567]
Robot Arm 1 togoal: 10.1390317484568
Mobile Platform 2 Pose: [-2.35332401041999;0.161435790444566;-9.59897514288128]
Robot Arm 2 end: [0.305117749128415;-9.59698100176271;1.31559587375624]
Robot Arm 1 togoal: 10.3150408138514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814885e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.819948738754383;-0.224372611975435;10.2328108095555]
Robot Arm 1 end: [0.0773086796938127;10.2377343156826;1.24173201981812]
Robot Arm 1 togoal: 9.92946681588129
Mobile Platform 2 Pose: [-2.35332253670386;0.13716633467537;-9.88497430631687]
Robot Arm 2 end: [0.0777039713515931;-9.87582909639425;1.33529476180363]
Robot Arm 1 togoal: 10.0801796701741

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820741e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820425e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.819941346449612;-0.202023899391206;10.5787024022009]
Robot Arm 1 end: [-0.186531307029783;10.5903096153388;1.31221709357285]
Robot Arm 1 togoal: 10.2057569936323
Mobile Platform 2 Pose: [-2.35324604677459;0.0888410586349724;-9.66020039554871]
Robot Arm 2 end: [0.195467352202515;-9.65823810960243;1.32142051376664]
Robot Arm 1 togoal: 10.2031459702819

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815253e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.819949516585204;-0.177341781276937;10.2216906336911]
Robot Arm 1 end: [0.0650266201484578;10.2254286979445;1.2697907389851]
Robot Arm 1 togoal: 9.94070521889847
Mobile Platform 2 Pose: [-2.35358521311754;0.0564438902950241;-9.91923128461324]
Robot Arm 2 end: [-0.0452867475031043;-9.90880492055012;1.32845628161098]
Robot Arm 1 togoal: 9.956992797298

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820805e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.819940564325505;-0.155017276945395;10.5965059923574]
Robot Arm 1 end: [-0.206734435255694;10.6099020357905;1.30907736051534]
Robot Arm 1 togoal: 10.2271364286501
Mobile Platform 2 Pose: [-2.35434866776745;-0.00129927146440096;-9.71287136948244]
Robot Arm 2 end: [0.13127245030254;-9.71258717935822;1.31447939335927]
Robot Arm 1 togoal: 10.1374522614262

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814171e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.81995012096433;-0.130284143440275;10.2223386295326]
Robot Arm 1 end: [0.0580551776051617;10.2248625721129;1.28866002423148]
Robot Arm 1 togoal: 9.94720120899446
Mobile Platform 2 Pose: [-2.35547608770403;-0.06131075188022;-9.92055042421894]
Robot Arm 2 end: [-0.138941262844714;-9.91020755754732;1.33043062455363]
Robot Arm 1 togoal: 9.86330871397658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820753e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.822086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.819939680418364;-0.107940344509442;10.617316546931]
Robot Arm 1 end: [-0.209342359635267;10.632719632675;1.30145314926029]
Robot Arm 1 togoal: 10.2312856655205
Mobile Platform 2 Pose: [-2.35842445640736;-0.134529035265288;-9.77630166795073]
Robot Arm 2 end: [0.0790957278999507;-9.77995418563176;1.28873331901572]
Robot Arm 1 togoal: 10.0841726811716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814199e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816048e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.819950656891445;-0.0832142253649497;10.22856632981]
Robot Arm 1 end: [0.0542747705517169;10.2298159946601;1.301208273755]
Robot Arm 1 togoal: 9.95080771264381
Mobile Platform 2 Pose: [-2.36188629366524;-0.214942769750075;-9.87216917071805]
Robot Arm 2 end: [-0.203436040952625;-9.86305177222377;1.33333670908286]
Robot Arm 1 togoal: 9.79931822789498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.822600e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.819938699741372;-0.060831458151917;10.6442516847913]
Robot Arm 1 end: [-0.202575953403029;10.6619190558571;1.29179506665937]
Robot Arm 1 togoal: 10.226594145801
Mobile Platform 2 Pose: [-2.36579557780248;-0.287537633539457;-9.92871389652027]
Robot Arm 2 end: [0.0376552115302966;-9.93859032070886;1.23139834753229]
Robot Arm 1 togoal: 10.0420198372331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814294e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.8199511810315;-0.0361375728061542;10.239265919362]
Robot Arm 1 end: [0.0529873706037066;10.2391466844073;1.30877268891351]
Robot Arm 1 togoal: 9.95215011028287
Mobile Platform 2 Pose: [-2.36786567769178;-0.348110233453013;-9.72255954618943]
Robot Arm 2 end: [-0.19459563760568;-9.71545393229511;1.31157538871458]
Robot Arm 1 togoal: 9.81176741685818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.822199e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.819937625247773;-0.0137025474469899;10.6770388579914]
Robot Arm 1 end: [-0.189170795809238;10.6972317724368;1.28117611557851]
Robot Arm 1 togoal: 10.2158136800847
Mobile Platform 2 Pose: [-2.36787312117835;-0.370044178754929;-10.0633759574094]
Robot Arm 2 end: [0.039452604912723;-10.0796378174647;1.16459052623397]
Robot Arm 1 togoal: 10.046092698221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814430e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.819951736892598;0.010944000762808;10.2531111955969]
Robot Arm 1 end: [0.0540781044168589;10.2514907733994;1.31216145515416]
Robot Arm 1 togoal: 9.95129255423427
Mobile Platform 2 Pose: [-2.36786518238573;-0.394131746842007;-9.7115798491052]
Robot Arm 2 end: [-0.153142879208148;-9.70525535514978;1.27945778567501]
Robot Arm 1 togoal: 9.85422813843165
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 91)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',91,0)">第 91 行</a>)
    real_time_2D_plot(human_poses{k}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.051621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.052167e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785389464117813;-9.90082571744751;2.36162035136777]
Robot Arm 1 end: [-9.16528507971623;2.36373825415995;1.02051715343198]
Robot Arm 1 togoal: 20.6366452873882
Mobile Platform 2 Pose: [-2.35689133400222;9.90082571714129;-2.36162035170728]
Robot Arm 2 end: [9.16561981382597;-2.33933145319951;1.02051715343198]
Robot Arm 1 togoal: 20.6459998186517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.472817e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.458669e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785385007499394;-9.82700709617448;1.73789135919245]
Robot Arm 1 end: [-10.0025043826277;1.73814075992777;-0.0599870974939367]
Robot Arm 1 togoal: 21.6992631184161
Mobile Platform 2 Pose: [-2.35575933398851;9.80068672442553;-1.19431051848964]
Robot Arm 2 end: [9.36042513831177;-1.18494643281635;0.0572832350332704]
Robot Arm 1 togoal: 21.3230789919405

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.347330e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.610178e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785390678940117;-9.72384267710847;6.05793733065442]
Robot Arm 1 end: [-10.5236488963453;6.05563305715669;0.839023741991828]
Robot Arm 1 togoal: 20.9103631297196
Mobile Platform 2 Pose: [-2.35575429642489;9.69987845164039;-7.02092918686661]
Robot Arm 2 end: [10.2523428564546;-7.031240560077;0.134268455536186]
Robot Arm 1 togoal: 20.5157000407963

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785387961755747;-9.67765864982136;6.60741398992723]
Robot Arm 1 end: [-9.79781506213353;6.60742029266867;1.51160391517129]
Robot Arm 1 togoal: 20.08639257287
Mobile Platform 2 Pose: [-2.35575796858822;9.65288300391151;-6.61519113212542]
Robot Arm 2 end: [10.329134380465;-6.63371893914314;1.15876227721058]
Robot Arm 1 togoal: 20.609142849898

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.251157e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.446970e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.78539265778002;-9.58228019638967;8.46002618987666]
Robot Arm 1 end: [-9.00208001065773;8.46008497045566;1.25765175233316]
Robot Arm 1 togoal: 19.0661935196621
Mobile Platform 2 Pose: [-2.3549908311303;9.55446948104995;-7.77810533051016]
Robot Arm 2 end: [9.37879723371105;-7.77558877819554;1.49581406625031]
Robot Arm 1 togoal: 19.5060611564774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.660112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.573429e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785390197076815;-9.5378367217076;8.97505420311998]
Robot Arm 1 end: [-8.77766216998984;8.96897233404717;0.516229299947008]
Robot Arm 1 togoal: 18.8327687496532
Mobile Platform 2 Pose: [-2.35458882705039;9.46554274082958;-7.99077001613387]
Robot Arm 2 end: [8.7138240163616;-7.97502529210815;0.949167798562133]
Robot Arm 1 togoal: 18.8317477667251

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.354433e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.348074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.78538704244426;-9.49259894282497;9.5736749503962]
Robot Arm 1 end: [-9.34956166524899;9.56788666694782;-0.05967285009647]
Robot Arm 1 togoal: 19.418825536806
Mobile Platform 2 Pose: [-2.35458626875048;9.42079218199757;-8.53010908388579]
Robot Arm 2 end: [8.8503085219027;-8.52435252256711;0.179867761056192]
Robot Arm 1 togoal: 18.9554821234097

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.917808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.957080e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785385306918771;-9.4070623277293;9.34807748819013]
Robot Arm 1 end: [-10.1619427754377;9.35621027495601;0.447443969673844]
Robot Arm 1 togoal: 20.2007654369914
Mobile Platform 2 Pose: [-2.35458332955899;9.37463850745331;-9.03335840412636]
Robot Arm 2 end: [9.72774883534169;-9.04177808098233;0.00700029683735764]
Robot Arm 1 togoal: 19.8089489488359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.776454e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.644609e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785390686470576;-9.36066090382139;9.70436835946852]
Robot Arm 1 end: [-9.82711280373458;9.70314010149039;1.3656500262038]
Robot Arm 1 togoal: 19.8299435588132
Mobile Platform 2 Pose: [-2.35458754630082;9.32780787304879;-9.77836253841137]
Robot Arm 2 end: [10.1018451881297;-9.79698677927643;0.917497487058113]
Robot Arm 1 togoal: 20.1119270488546

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.813606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.619275e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785387722050751;-9.31597985428582;10.3689073562621]
Robot Arm 1 end: [-9.00806047096723;10.3623692644379;1.43937607886057]
Robot Arm 1 togoal: 19.0116894782199
Mobile Platform 2 Pose: [-2.35458688318046;9.28234164841967;-9.28987187897927]
Robot Arm 2 end: [9.41810288734018;-9.29601652508622;1.49509151633246]
Robot Arm 1 togoal: 19.4308770704209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.332611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.316189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785389723712604;-9.27175471659759;10.7301387332371]
Robot Arm 1 end: [-8.51412027566245;10.7233826571358;0.86920804019121]
Robot Arm 1 togoal: 18.5397077865109
Mobile Platform 2 Pose: [-2.35746910571737;9.13659061415617;-9.39309972207081]
Robot Arm 2 end: [8.56698759693857;-9.38225233265424;1.2420520448007]
Robot Arm 1 togoal: 18.5793555476796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.444713e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.459836e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.789363581504031;-9.12393557780962;10.7780364740007]
Robot Arm 1 end: [-8.58557221946867;10.7660503384782;0.168225510112407]
Robot Arm 1 togoal: 18.6504779205965
Mobile Platform 2 Pose: [-2.35969663738122;9.04377131364841;-9.19112708732396]
Robot Arm 2 end: [8.28374965769425;-9.18196998221194;0.630649417447201]
Robot Arm 1 togoal: 18.3236840950473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.524474e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.546950e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.791385218244272;-9.07302354894908;10.4935629305309]
Robot Arm 1 end: [-9.33291954580318;10.4897870828172;-0.0133253326812131]
Robot Arm 1 togoal: 19.3998923651009
Mobile Platform 2 Pose: [-2.35969408317741;8.99979121732119;-9.53427335256034]
Robot Arm 2 end: [8.61867623758501;-9.5377699353579;0.0556767004328841]
Robot Arm 1 togoal: 18.681968146984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.587068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.600490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.791387535353743;-9.02671522015434;10.0624207019419]
Robot Arm 1 end: [-9.80671037350324;10.0655252767529;0.758305295286573]
Robot Arm 1 togoal: 19.8214977384236
Mobile Platform 2 Pose: [-2.35969161442476;8.95442273841104;-9.84815555469541]
Robot Arm 2 end: [9.40446606648581;-9.86105363205376;0.0915444294157686]
Robot Arm 1 togoal: 19.4575423074662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.697231e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.667130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.7913898496858;-8.98152213913916;10.5401243861569]
Robot Arm 1 end: [-9.29436938033109;10.5369768798938;1.42236303119444]
Robot Arm 1 togoal: 19.3020921976301
Mobile Platform 2 Pose: [-2.35969635402356;8.90839557650474;-10.3420320694727]
Robot Arm 2 end: [9.64878506467376;-10.3619926540541;0.936756669173153]
Robot Arm 1 togoal: 19.6608019538612

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.795770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.775291e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.791390318046769;-8.93750759785693;10.8933677905553]
Robot Arm 1 end: [-8.57124233348082;10.8862121417743;1.37773370483546]
Robot Arm 1 togoal: 18.5929271229796
Mobile Platform 2 Pose: [-2.35969579303871;8.86383335154103;-9.7153244968362]
Robot Arm 2 end: [9.0757564090942;-9.72580643694147;1.44797590133335]
Robot Arm 1 togoal: 19.0778666838855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.871635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.857002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.790900868358201;-8.82575239616245;11.0597220297263]
Robot Arm 1 end: [-8.10433166177737;11.0494777870345;0.888669573452911]
Robot Arm 1 togoal: 18.1457452399578
Mobile Platform 2 Pose: [-2.36037505421385;8.72136410921404;-9.60492096512026]
Robot Arm 2 end: [8.29978414730982;-9.59865493298088;1.33039243334817]
Robot Arm 1 togoal: 18.3051771077315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.930886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.938449e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.790217563760674;-8.69751872442586;10.9246524160125]
Robot Arm 1 end: [-8.07722081376587;10.9092933049809;0.325021111467084]
Robot Arm 1 togoal: 18.1395449823293
Mobile Platform 2 Pose: [-2.36104135982445;8.64414715304089;-9.37681119217932]
Robot Arm 2 end: [7.94023664391467;-9.37006342790786;0.91475298025099]
Robot Arm 1 togoal: 17.9615268385923

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.969327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.984940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.789346867245577;-8.57591935947007;10.7758195822028]
Robot Arm 1 end: [-8.47993918507456;10.761682790251;-0.00996894971026813]
Robot Arm 1 togoal: 18.5588840958177
Mobile Platform 2 Pose: [-2.36207804128482;8.57321512020313;-9.61683453896076]
Robot Arm 2 end: [7.92009317807599;-9.6144950574055;0.408718838628841]
Robot Arm 1 togoal: 17.9587171856353

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.996028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.017949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.789345706942285;-8.53133721192736;10.3920343573217]
Robot Arm 1 end: [-9.10179760075315;10.3907638838338;0.24956763730664]
Robot Arm 1 togoal: 19.1480523355758
Mobile Platform 2 Pose: [-2.36439613421687;8.42932352849843;-9.59434352713369]
Robot Arm 2 end: [8.19738856685419;-9.60475603007478;0.0360713889352023]
Robot Arm 1 togoal: 18.2621516100594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.030765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.045428e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.788240264987371;-8.46149135049637;10.143100039715]
Robot Arm 1 end: [-9.14121274376456;10.1410182597407;0.994884583466727]
Robot Arm 1 togoal: 19.1489610392613
Mobile Platform 2 Pose: [-2.36439413714113;8.3854304392039;-9.99808538343299]
Robot Arm 2 end: [8.78159967360364;-10.0151176711995;0.114161895762273]
Robot Arm 1 togoal: 18.8342217331916

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.096450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.068890e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.788240378259435;-8.4178665622393;10.721465431139]
Robot Arm 1 end: [-8.65213010582933;10.7149342086314;1.3989266651949]
Robot Arm 1 togoal: 18.6662259256264
Mobile Platform 2 Pose: [-2.364397161328;8.34068828334578;-10.3101306573059]
Robot Arm 2 end: [9.06073452220317;-10.3308397866309;0.74749259814176]
Robot Arm 1 togoal: 19.0792916323184

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.142293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.120960e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.787047496792229;-8.29018346204073;10.8572588762162]
Robot Arm 1 end: [-7.99324722415616;10.8508573065258;1.35457399179626]
Robot Arm 1 togoal: 18.0141222778652
Mobile Platform 2 Pose: [-2.36439928157834;8.29678759178674;-9.85696153452282]
Robot Arm 2 end: [8.71686271225294;-9.87383764014522;1.28880021363447]
Robot Arm 1 togoal: 18.7187281477978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.195154e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.171367e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785845659645376;-8.20935522242097;11.0765496003522]
Robot Arm 1 end: [-7.6265573554149;11.0646447066992;1.08285910414575]
Robot Arm 1 togoal: 17.6641150358532
Mobile Platform 2 Pose: [-2.36568230808244;8.23512057606914;-9.59890254739252]
Robot Arm 2 end: [8.15940982937632;-9.60481537285047;1.40558069892121]
Robot Arm 1 togoal: 18.1640760253817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.226331e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.215940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.784672165865818;-8.1306189441842;10.8539063865969]
Robot Arm 1 end: [-7.45556787597725;10.8415595172122;0.724240645850496]
Robot Arm 1 togoal: 17.4939960489959
Mobile Platform 2 Pose: [-2.36690392319808;8.1213145440633;-9.44021982239428]
Robot Arm 2 end: [7.67692476649109;-9.4377196660111;1.22625858331086]
Robot Arm 1 togoal: 17.6883210304678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.253149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.253632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.783545702227125;-8.00066498917516;10.9812656343565]
Robot Arm 1 end: [-7.4271156263603;10.9638830628121;0.380558745060407]
Robot Arm 1 togoal: 17.4909701237317
Mobile Platform 2 Pose: [-2.36803922564179;7.9872305850741;-9.5593045190332]
Robot Arm 2 end: [7.3660482530632;-9.55353310264342;0.943391575442275]
Robot Arm 1 togoal: 17.3813864841178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.274971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.284201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.783543377096478;-7.95896977564508;10.6669789959356]
Robot Arm 1 end: [-7.65786802739001;10.6471078177211;0.132104311820755]
Robot Arm 1 togoal: 17.7242230586596
Mobile Platform 2 Pose: [-2.37009216813998;7.85079429663447;-9.50968040832889]
Robot Arm 2 end: [7.20491043069436;-9.50665776623073;0.657868855433826]
Robot Arm 1 togoal: 17.233610325772

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.285218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.308588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.781653228654205;-7.81726553245091;10.6336759987779]
Robot Arm 1 end: [-7.95747555165894;10.6187308414118;0.077425010396435]
Robot Arm 1 togoal: 18.026027426839
Mobile Platform 2 Pose: [-2.37098860790691;7.77311999479015;-9.73197978480166]
Robot Arm 2 end: [7.21665903422679;-9.7352453191158;0.405410998688634]
Robot Arm 1 togoal: 17.254795917707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.301613e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.324644e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.781653399200999;-7.77436723277977;10.286121756489]
Robot Arm 1 end: [-8.30855328820267;10.2805321763103;0.342799262596489]
Robot Arm 1 togoal: 18.3485688484557
Mobile Platform 2 Pose: [-2.37249820459052;7.63609980989913;-9.67908100508763]
Robot Arm 2 end: [7.28568982665773;-9.69067762064172;0.194842138666008]
Robot Arm 1 togoal: 17.3392458829953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.317826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.341635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.781659791612537;-7.67717347030719;10.4791630371739]
Robot Arm 1 end: [-8.31183871333445;10.4750356852425;0.832185627858304]
Robot Arm 1 togoal: 18.3309454531046
Mobile Platform 2 Pose: [-2.37298523353047;7.59401722915458;-9.96907835818608]
Robot Arm 2 end: [7.53627920464678;-9.98759370993651;0.0978701813599315]
Robot Arm 1 togoal: 17.5939347713939

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.349096e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.349398e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.781664110128042;-7.63096464636642;10.7646428692859]
Robot Arm 1 end: [-8.05012576587781;10.7577497545471;1.18903355675639]
Robot Arm 1 togoal: 18.0690737608202
Mobile Platform 2 Pose: [-2.37386215793248;7.45375991949954;-9.96271296728462]
Robot Arm 2 end: [7.76290863001921;-9.983652240547;0.182877387685898]
Robot Arm 1 togoal: 17.813246822891

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.375857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.366319e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.78166783741303;-7.53387829344615;10.9539082228468]
Robot Arm 1 end: [-7.62719368422773;10.9456430915547;1.32364913646999]
Robot Arm 1 togoal: 17.6536439694817
Mobile Platform 2 Pose: [-2.37200467481222;7.40394753166169;-10.2407334069619]
Robot Arm 2 end: [7.96240530220305;-10.2617968409453;0.468874358687303]
Robot Arm 1 togoal: 17.995096837518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.405303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.382268e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.781669220858965;-7.39746760820381;11.015938209174]
Robot Arm 1 end: [-7.23396447580943;11.0070380915699;1.29033521524882]
Robot Arm 1 togoal: 17.2649026500082
Mobile Platform 2 Pose: [-2.37200720067257;7.36164930022;-9.89100329945887]
Robot Arm 2 end: [7.94783715941073;-9.91246387057643;0.849912323426814]
Robot Arm 1 togoal: 17.9605924162965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.429393e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.404638e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.781671214277709;-7.26347027351964;10.9265091234654]
Robot Arm 1 end: [-6.93655593317376;10.918154211736;1.19710419333601]
Robot Arm 1 togoal: 16.9645171616414
Mobile Platform 2 Pose: [-2.36996699929103;7.23868840354936;-9.74975190733665]
Robot Arm 2 end: [7.64939386924819;-9.76840222936433;1.13553465409882]
Robot Arm 1 togoal: 17.6551217784559

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.438461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.432751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.781670411864646;-7.13201957237358;10.9368650490795]
Robot Arm 1 end: [-6.83683245540043;10.9277619498879;1.21410930363718]
Robot Arm 1 togoal: 16.865166802211
Mobile Platform 2 Pose: [-2.36789730938978;7.18028286682986;-9.491540953878]
Robot Arm 2 end: [7.38166358282885;-9.50536992056079;1.2496517735487]
Robot Arm 1 togoal: 17.3908170504471

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.461091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.437693e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.781672566867555;-7.0205002083751;10.7839430992256]
Robot Arm 1 end: [-6.87029416176087;10.7769924560273;1.28680441776662]
Robot Arm 1 togoal: 16.8898013591577
Mobile Platform 2 Pose: [-2.36534647227772;7.04815161123132;-9.60923643654874]
Robot Arm 2 end: [7.18682300288604;-9.62115038754766;1.25471491449468]
Robot Arm 1 togoal: 17.1930602194069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.464864e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.466308e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.781671824882636;-6.90613847213138;10.9335235014365]
Robot Arm 1 end: [-6.83895965947646;10.9259134939338;1.30590895219744]
Robot Arm 1 togoal: 16.8657683599227
Mobile Platform 2 Pose: [-2.36336533125098;6.93811044037643;-9.45235497443257]
Robot Arm 2 end: [7.19734501093353;-9.46686030188;1.22295803713276]
Robot Arm 1 togoal: 17.2081882362561

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.482542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.466590e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.781673717031293;-6.84386282572588;10.6888302791716]
Robot Arm 1 end: [-6.9346850253803;10.6823988008716;1.3122983980566]
Robot Arm 1 togoal: 16.949713312942
Mobile Platform 2 Pose: [-2.36151472916248;6.86254490807917;-9.67609175815356]
Robot Arm 2 end: [7.13219684764053;-9.69094171843337;1.21618567377062]
Robot Arm 1 togoal: 17.137695224889

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.487948e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.487010e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.781671975620889;-6.77775602175952;10.9277293432786]
Robot Arm 1 end: [-6.97697791552775;10.9204509081653;1.29182311582183]
Robot Arm 1 togoal: 17.0034564445851
Mobile Platform 2 Pose: [-2.35985182512843;6.75440734767046;-9.51522470413287]
Robot Arm 2 end: [7.12368529326143;-9.53245209922933;1.16282996894505]
Robot Arm 1 togoal: 17.1338111590305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.499546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.495260e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.781672837312152;-6.73878952342901;10.6305267316483]
Robot Arm 1 end: [-7.06058840801503;10.6238634191256;1.24412831441943]
Robot Arm 1 togoal: 17.074236156376
Mobile Platform 2 Pose: [-2.35844243716098;6.68703341107609;-9.75244934843127]
Robot Arm 2 end: [7.1197545392907;-9.77151171228629;1.1088374972724]
Robot Arm 1 togoal: 17.1262395262895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.507491e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.507941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.781671752651656;-6.69963437357351;10.9203369926656]
Robot Arm 1 end: [-7.11641369816586;10.9133378532729;1.18330675519654]
Robot Arm 1 togoal: 17.1440905459743
Mobile Platform 2 Pose: [-2.35734509969829;6.59070080204295;-9.5697167622513]
Robot Arm 2 end: [7.10240000479772;-9.59071400681947;1.01990481795483]
Robot Arm 1 togoal: 17.1146340109798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.516440e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.518358e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.781672343379631;-6.66042125754471;10.6081330524553]
Robot Arm 1 end: [-7.16048029733589;10.6013761842569;1.10814316982064]
Robot Arm 1 togoal: 17.1759770642741
Mobile Platform 2 Pose: [-2.35661216671137;6.52263500220548;-9.804806037441]
Robot Arm 2 end: [7.09223844235175;-9.8270860077646;0.913032304687841]
Robot Arm 1 togoal: 17.1039217414446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.525907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.529174e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.781671365554421;-6.62121352699403;10.9018508488916]
Robot Arm 1 end: [-7.19020536496476;10.8954365271364;1.01769847386341]
Robot Arm 1 togoal: 17.2208675591564
Mobile Platform 2 Pose: [-2.35661095203538;6.43953577759165;-9.59806969817255]
Robot Arm 2 end: [7.0498516528788;-9.62105514924316;0.775824248412505]
Robot Arm 1 togoal: 17.0703347209668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.534396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.540237e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.781671360057388;-6.58196216234738;10.5944267140212]
Robot Arm 1 end: [-7.20320621228446;10.5883360026545;0.91817814183408]
Robot Arm 1 togoal: 17.2238159949265
Mobile Platform 2 Pose: [-2.35660831299728;6.3791327358716;-9.84640534499621]
Robot Arm 2 end: [6.99315903152006;-9.86891076661488;0.612626123142735]
Robot Arm 1 togoal: 17.0179252953407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.543983e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.551373e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.781669614415175;-6.54193681958794;10.8820068797743]
Robot Arm 1 end: [-7.19866073918349;10.8766513199212;0.800954119944601]
Robot Arm 1 togoal: 17.2360353569966
Mobile Platform 2 Pose: [-2.35660513260434;6.30927265934167;-9.61635958344889]
Robot Arm 2 end: [6.87285233116866;-9.63723900521368;0.425645254015228]
Robot Arm 1 togoal: 16.9122601490254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.552742e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.564423e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.781668944417129;-6.50280860957467;10.5823870706761]
Robot Arm 1 end: [-7.17278506121083;10.5777786477352;0.672288685400992]
Robot Arm 1 togoal: 17.2034498232143
Mobile Platform 2 Pose: [-2.35660061041732;6.27048952439399;-9.90278274259271]
Robot Arm 2 end: [6.6922554677585;-9.92109938576715;0.22673412050783]
Robot Arm 1 togoal: 16.7425428795825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.562606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.577869e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.781666661293791;-6.45467924313544;10.8542941905901]
Robot Arm 1 end: [-7.10725676269271;10.8505495838532;0.522953206054094]
Robot Arm 1 togoal: 17.1574405417065
Mobile Platform 2 Pose: [-2.35659481631698;6.19483574572135;-9.68644300040366]
Robot Arm 2 end: [6.35006676432804;-9.70320292169384;0.0740774513231641]
Robot Arm 1 togoal: 16.4166487617158

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.572736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.596158e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.781665216389238;-6.41596886594936;10.5596277125684]
Robot Arm 1 end: [-7.00441376885;10.5564001542343;0.355506846615808]
Robot Arm 1 togoal: 17.0533880224568
Mobile Platform 2 Pose: [-2.35659259703299;6.15852442266201;-10.0278116443583]
Robot Arm 2 end: [5.8907367760278;-10.0464690348071;0.0933273494883549]
Robot Arm 1 togoal: 15.9548088002714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.583856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.611769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.781660800969949;-6.35609027274816;10.8079473552946]
Robot Arm 1 end: [-6.80633993991722;10.803725032313;0.179569419259489]
Robot Arm 1 togoal: 16.8789357515979
Mobile Platform 2 Pose: [-2.35659614019976;6.04047190797827;-10.0751915586665]
Robot Arm 2 end: [5.39547348069804;-10.0906122070682;0.453924236699577]
Robot Arm 1 togoal: 15.4326752363798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.597396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622932e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.781658095112887;-6.31842329007016;10.4992248190592]
Robot Arm 1 end: [-6.50758342492735;10.4893404531459;0.0293337825180266]
Robot Arm 1 togoal: 16.582063584319
Mobile Platform 2 Pose: [-2.35660938286112;5.92412158585473;-10.0302988638974]
Robot Arm 2 end: [5.29077142690817;-10.0337237554814;1.03486934310617]
Robot Arm 1 togoal: 15.2985342806822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.612119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622205e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.78165227595896;-6.19675807910274;10.6284919148926]
Robot Arm 1 end: [-5.98734481163076;10.6045674112294;0.0205442221431839]
Robot Arm 1 togoal: 16.0689782942788
Mobile Platform 2 Pose: [-2.35661069255985;5.88856599090861;-9.63121799983987]
Robot Arm 2 end: [5.64106741002534;-9.63747148481665;1.38565022182491]
Robot Arm 1 togoal: 15.645853642794

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.631377e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.617396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.78165253346383;-6.16130222111432;10.2553921262947]
Robot Arm 1 end: [-5.51146366007522;10.2175188492896;0.357515165727524]
Robot Arm 1 togoal: 15.5565586260976
Mobile Platform 2 Pose: [-2.35661575784446;5.77394183921788;-9.4894162537971]
Robot Arm 2 end: [5.93856650988327;-9.50066668130002;1.41310361963088]
Robot Arm 1 togoal: 15.946751323766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.639984e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622226e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.781665048428632;-6.07265806134083;10.4411192570672]
Robot Arm 1 end: [-5.35588166237314;10.4177894554373;0.951168868427231]
Robot Arm 1 togoal: 15.3721292336615
Mobile Platform 2 Pose: [-2.35661257397963;5.65144928797969;-9.36936525892037]
Robot Arm 2 end: [6.16846897306126;-9.38855440647747;1.21355154822912]
Robot Arm 1 togoal: 16.1829471727859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.639948e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.627159e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.781666237476204;-6.0369740228391;10.8268861921128]
Robot Arm 1 end: [-5.65711442098316;10.8117923700082;1.37257594956312]
Robot Arm 1 togoal: 15.6788478065995
Mobile Platform 2 Pose: [-2.3566127496757;5.5424020207635;-9.52207553703355]
Robot Arm 2 end: [6.24854198881251;-9.54516100749881;0.859588285301315]
Robot Arm 1 togoal: 16.2683576530528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.635176e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.635422e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.781675399128666;-5.95359752435712;11.0270566262071]
Robot Arm 1 end: [-5.98565932550609;11.0216922001721;1.47497867421386]
Robot Arm 1 togoal: 16.0183418925137
Mobile Platform 2 Pose: [-2.35660624691949;5.42059352146276;-9.42557703378811]
Robot Arm 2 end: [6.08573518930327;-9.44563714156413;0.424141590305192]
Robot Arm 1 togoal: 16.1326158179671

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.637485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.649506e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.781671664669172;-5.8349626340497;11.1627155416908]
Robot Arm 1 end: [-6.23613899894693;11.1590002890821;1.36058646068924]
Robot Arm 1 togoal: 16.2782438784659
Mobile Platform 2 Pose: [-2.35659999272074;5.38422742521271;-9.70971945599747]
Robot Arm 2 end: [5.66388890651677;-9.72231080560872;0.0334968840182269]
Robot Arm 1 togoal: 15.7368101114691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.641146e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.665646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.781676552212437;-5.75147999804897;10.9612753245406]
Robot Arm 1 end: [-6.41042391976896;10.9577305133532;1.09432094500304]
Robot Arm 1 togoal: 16.4438838471544
Mobile Platform 2 Pose: [-2.35659418119558;5.27186753521996;-9.84920666925639]
Robot Arm 2 end: [4.8651677678974;-9.85875711166075;0.0798865814632205]
Robot Arm 1 togoal: 14.9355271151148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.645813e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.683068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.781672298789037;-5.63103793426123;11.0942533375368]
Robot Arm 1 end: [-6.39187552296794;11.0939960904432;0.744443010061022]
Robot Arm 1 togoal: 16.4466851178286
Mobile Platform 2 Pose: [-2.3565977042358;5.23895353049002;-10.2302284992057]
Robot Arm 2 end: [4.47938068959835;-10.2284456352121;0.846939561980671]
Robot Arm 1 togoal: 14.4968620755205

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.655093e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.676461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.781670877702674;-5.59419638776739;10.7942953611108]
Robot Arm 1 end: [-6.25530791431282;10.7958446076424;0.329994632454006]
Robot Arm 1 togoal: 16.3182994767886
Mobile Platform 2 Pose: [-2.35659952674002;5.20484119822627;-9.76604860520804]
Robot Arm 2 end: [4.88816337823183;-9.76718636391396;1.42125316055915]
Robot Arm 1 togoal: 14.8903176728386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.665494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.670734e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.781660911475949;-5.47355224187841;10.907227774688]
Robot Arm 1 end: [-5.75583117694188;10.9064974399925;-0.00425906447115926]
Robot Arm 1 togoal: 15.8554208048465
Mobile Platform 2 Pose: [-2.35660272936492;5.12949341572432;-9.55528890292046]
Robot Arm 2 end: [5.41956873709743;-9.56719948534503;1.42876397633002]
Robot Arm 1 togoal: 15.4259172760527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.684165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.671700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.781657963008731;-5.43901723757877;10.5306959935647]
Robot Arm 1 end: [-5.02381851297488;10.513122188536;0.0466002469909759]
Robot Arm 1 togoal: 15.1047102495496
Mobile Platform 2 Pose: [-2.35660147515973;5.02820392032293;-9.39158023258965]
Robot Arm 2 end: [5.72435278180343;-9.41166733582592;1.0357499749638]
Robot Arm 1 togoal: 15.7428356186915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.697736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.677966e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.781661582309505;-5.40561148652376;10.1935228745898]
Robot Arm 1 end: [-4.60782710808293;10.1752494427668;0.780390332646994]
Robot Arm 1 togoal: 14.6276391761012
Mobile Platform 2 Pose: [-2.35659833849538;4.91040064989684;-9.52396439561696]
Robot Arm 2 end: [5.62642833138676;-9.54186583401342;0.440311791818605]
Robot Arm 1 togoal: 15.6704510564729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.694936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.691705e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.781664074704513;-5.37143260999869;10.6257397833949]
Robot Arm 1 end: [-4.97181923607508;10.6179121427735;1.41033933916814]
Robot Arm 1 togoal: 14.9849735547231
Mobile Platform 2 Pose: [-2.35658772728515;4.79616045370676;-9.52669012384053]
Robot Arm 2 end: [4.96806563988531;-9.5337972356099;-0.041641722459461]
Robot Arm 1 togoal: 15.0566325308885

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.687872e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.712007e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.781665765231523;-5.33595033486457;10.916142677466]
Robot Arm 1 end: [-5.53024107397095;10.9143668946875;1.48749318201267]
Robot Arm 1 togoal: 15.5571712512464
Mobile Platform 2 Pose: [-2.35658641958721;4.76386120369495;-9.93115827001564]
Robot Arm 2 end: [4.07084321405429;-9.93242058480953;0.326375273204352]
Robot Arm 1 togoal: 14.1216261092598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.686308e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.715483e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.781666374700074;-5.21543199553247;11.0395017150301]
Robot Arm 1 end: [-5.84143291884988;11.0403106165743;1.19764421982092]
Robot Arm 1 togoal: 15.8788476301636
Mobile Platform 2 Pose: [-2.35719392316985;4.73062199572926;-10.2124591808754]
Robot Arm 2 end: [4.14882174202804;-10.2012438106778;1.26717845997203]
Robot Arm 1 togoal: 14.1525291426375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.691354e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.705485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.781664162986289;-5.09536962661988;10.9499640977101]
Robot Arm 1 end: [-5.88115468892712;10.9535723053933;0.683868534438573]
Robot Arm 1 togoal: 15.9317658621346
Mobile Platform 2 Pose: [-2.35719025515386;4.69671889609386;-9.639335521053]
Robot Arm 2 end: [4.8565150086977;-9.64594835860999;1.49446817275833]
Robot Arm 1 togoal: 14.8607568685544

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.701635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.702288e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.781656350917996;-4.97756947506459;10.9257354781949]
Robot Arm 1 end: [-5.49819638080926;10.9316593162671;0.12394433212494]
Robot Arm 1 togoal: 15.5889013300764
Mobile Platform 2 Pose: [-2.35632436931623;4.58571589329599;-9.49628977657493]
Robot Arm 2 end: [5.28939548465174;-9.51586389698983;1.0688956942202]
Robot Arm 1 togoal: 15.3037381170625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.718640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.709093e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.781653072957421;-4.94379678830951;10.6298285917804]
Robot Arm 1 end: [-4.68184192333812;10.6241532034234;-0.0393078973943539]
Robot Arm 1 togoal: 14.7777065276688
Mobile Platform 2 Pose: [-2.35410062577949;4.4713045464985;-9.47272883289169]
Robot Arm 2 end: [5.15802358939303;-9.4865434698584;0.335644894878662]
Robot Arm 1 togoal: 15.2129676089242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.734153e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.725140e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.781655465735082;-4.91146467675441;10.237197908789]
Robot Arm 1 end: [-4.10048133436035;10.2247457602246;0.741147406551393]
Robot Arm 1 togoal: 14.1238186969705
Mobile Platform 2 Pose: [-2.35277283918376;4.40043403217189;-9.68474597449453]
Robot Arm 2 end: [4.27657016072451;-9.68474654803272;-0.0704073565449944]
Robot Arm 1 togoal: 14.3684521667274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.730163e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.743092e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.78165853082274;-4.87824227247806;10.607180743631]
Robot Arm 1 end: [-4.54880312430893;10.604821959205;1.45398381037427]
Robot Arm 1 togoal: 14.5615237225336
Mobile Platform 2 Pose: [-2.35277495592342;4.36942297321084;-10.1110016149837]
Robot Arm 2 end: [3.56000007139765;-10.1002386937277;0.767359863415115]
Robot Arm 1 togoal: 13.5812968153817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.722554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.735852e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.781658662518831;-4.84363689325799;10.9107411249695]
Robot Arm 1 end: [-5.21884975600623;10.9136014378066;1.41949261414598]
Robot Arm 1 togoal: 15.2465851662737
Mobile Platform 2 Pose: [-2.35277750732479;4.33701128193416;-9.74877408361871]
Robot Arm 2 end: [4.13110171430413;-9.74514536383544;1.49611359684895]
Robot Arm 1 togoal: 14.1334215913432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.721379e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.729846e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.781659642612615;-4.72637020374388;11.0195113613747]
Robot Arm 1 end: [-5.48947517045646;11.0250902071198;0.912143818285644]
Robot Arm 1 togoal: 15.5352939089974
Mobile Platform 2 Pose: [-2.35094071985956;4.29836399661248;-9.47444404359263]
Robot Arm 2 end: [4.87331057382789;-9.48645651208059;1.26419704003234]
Robot Arm 1 togoal: 14.8843892081527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.728788e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.731939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.781653015999237;-4.61099432063164;10.9077340284996]
Robot Arm 1 end: [-5.22370634191088;10.9173014753268;0.225705103388404]
Robot Arm 1 togoal: 15.3062231020772
Mobile Platform 2 Pose: [-2.34705240229833;4.18583813534476;-9.37213850108497]
Robot Arm 2 end: [4.93753446217737;-9.38439275398537;0.481777660938338]
Robot Arm 1 togoal: 14.9862901066304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.743474e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.745853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.781646123840011;-4.53146822345264;10.7110309159761]
Robot Arm 1 end: [-4.31273894748291;10.7114876114635;-0.0465192007953986]
Robot Arm 1 togoal: 14.4158880144416
Mobile Platform 2 Pose: [-2.34503153081107;4.12368607111201;-9.59898267227537]
Robot Arm 2 end: [4.09998569578242;-9.59606117119674;-0.078280230259172]
Robot Arm 1 togoal: 14.1961424497496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.758751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.762479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.781648611004568;-4.50013958649356;10.2624852298914]
Robot Arm 1 end: [-3.70393372812062;10.2557108413828;0.824333639928518]
Robot Arm 1 togoal: 13.7240129581831
Mobile Platform 2 Pose: [-2.34503331258662;4.09326373313794;-10.034819651657]
Robot Arm 2 end: [3.28513502730625;-10.0213464980458;0.742793457040247]
Robot Arm 1 togoal: 13.3079252248517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.751567e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.753992e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.781650917692566;-4.46750065792029;10.6624047998255]
Robot Arm 1 end: [-4.29171750562511;10.6639565175292;1.49165688270915]
Robot Arm 1 togoal: 14.3071621412596
Mobile Platform 2 Pose: [-2.34503621439647;4.06138334816332;-9.70958430028674]
Robot Arm 2 end: [3.89835710597396;-9.70455898175109;1.50096655755445]
Robot Arm 1 togoal: 13.9015113199701

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.744979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.747945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.78165120648793;-4.42740351549197;10.9348089218958]
Robot Arm 1 end: [-4.98832362113386;10.9416920908009;1.257986779652]
Robot Arm 1 togoal: 15.0201799230167
Mobile Platform 2 Pose: [-2.34319612901668;4.02539452341339;-9.43182709916432]
Robot Arm 2 end: [4.668829841535;-9.44197035959136;1.16754368202634]
Robot Arm 1 togoal: 14.6836949494396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.746075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.751335e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.781648077680168;-4.31365126013124;11.0008235159482]
Robot Arm 1 end: [-5.06184630159781;11.0123197897781;0.536691025519849]
Robot Arm 1 togoal: 15.1278838416672
Mobile Platform 2 Pose: [-2.3395287364154;3.9155973475969;-9.36578171835019]
Robot Arm 2 end: [4.56441030509685;-9.37143745129434;0.285625278152153]
Robot Arm 1 togoal: 14.6302183550427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.757537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.767340e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.78164190654946;-4.2308575891118;10.8106550398356]
Robot Arm 1 end: [-4.32998254893573;10.8200500176036;-0.0490610374177958]
Robot Arm 1 togoal: 14.4390433736239
Mobile Platform 2 Pose: [-2.33952593172259;3.88454031830747;-9.67193251572867]
Robot Arm 2 end: [3.50091838881049;-9.66247288805471;0.0265584662976483]
Robot Arm 1 togoal: 13.5875706949504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.773618e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.777081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.781641961170033;-4.19993321460248;10.4135337301629]
Robot Arm 1 end: [-3.43310798155609;10.4084991232;0.558516671898911]
Robot Arm 1 togoal: 13.4737387437442
Mobile Platform 2 Pose: [-2.33953156767741;3.85453667127703;-10.151304341112]
Robot Arm 2 end: [3.18533758532518;-10.1327116330186;1.14529132343703]
Robot Arm 1 togoal: 13.1913568910166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.770815e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.764580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.78166484147263;-4.09449775117141;10.4377404688016]
Robot Arm 1 end: [-3.79137146489915;10.440885156424;1.42972061870811]
Robot Arm 1 togoal: 13.7987187278181
Mobile Platform 2 Pose: [-2.33952873489485;3.82247971326511;-9.60200995685829]
Robot Arm 2 end: [4.05836923531048;-9.60076809523269;1.45950398098103]
Robot Arm 1 togoal: 14.0641712671051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.765335e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.763165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.781662562599539;-4.06176381119733;10.847432847297]
Robot Arm 1 end: [-4.60824465429694;10.8587481951111;1.23865365079717]
Robot Arm 1 togoal: 14.6361873383511
Mobile Platform 2 Pose: [-2.33883570178179;3.74864529629007;-9.3960014922619]
Robot Arm 2 end: [4.51222706796437;-9.40516470914945;0.727592584810858]
Robot Arm 1 togoal: 14.5460667818444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.765321e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.772927e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.781667393372993;-3.95151725995788;10.9848462266235]
Robot Arm 1 end: [-4.63231556219697;11.0001450852053;0.417623244819912]
Robot Arm 1 togoal: 14.7079022694279
Mobile Platform 2 Pose: [-2.33946023684084;3.65100972338644;-9.55469343211874]
Robot Arm 2 end: [3.77131166142648;-9.54761542192418;-0.0385394744857929]
Robot Arm 1 togoal: 13.866716989075

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.777791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789628e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.78185640860612;-3.91930874768457;10.7045027949246]
Robot Arm 1 end: [-3.72796302234077;10.7122567459151;-0.00705846868321042]
Robot Arm 1 togoal: 13.8310968870358
Mobile Platform 2 Pose: [-2.33946143941045;3.62162323028667;-9.98387997913401]
Robot Arm 2 end: [2.84587316116093;-9.96529073377379;0.72571776472677]
Robot Arm 1 togoal: 12.8705142026081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789405e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.780434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.781859690743838;-3.88917854752188;10.2640839121423]
Robot Arm 1 end: [-3.16930468856132;10.2650269692482;0.941243059870607]
Robot Arm 1 togoal: 13.1847238645419
Mobile Platform 2 Pose: [-2.33946432563619;3.59062496933412;-9.69285554485475]
Robot Arm 2 end: [3.52467972080514;-9.68638918586979;1.47703850497716]
Robot Arm 1 togoal: 13.5283866704845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.780896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.776069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.781860037516144;-3.85750089758641;10.7072829518295]
Robot Arm 1 end: [-3.92931145543237;10.7152954893804;1.45040220947313]
Robot Arm 1 togoal: 13.9478438945808
Mobile Platform 2 Pose: [-2.34011106141257;3.54933528060184;-9.42828284229625]
Robot Arm 2 end: [4.25641574699393;-9.43457286936157;0.944105328082136]
Robot Arm 1 togoal: 14.2792823788062

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.776990e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.782669e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.782795913804525;-3.79820797364443;10.9402349105809]
Robot Arm 1 end: [-4.49679029392517;10.9535121971374;0.876554333704021]
Robot Arm 1 togoal: 14.5424009212008
Mobile Platform 2 Pose: [-2.34140788984213;3.4451389157511;-9.464970099747]
Robot Arm 2 end: [3.74001378837513;-9.45840258769665;0.0401323739376195]
Robot Arm 1 togoal: 13.8301943438143

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.783176e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798994e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785101197871696;-3.6917598406828;10.855142666437]
Robot Arm 1 end: [-3.99056827531461;10.870677683137;0.0717455534308354]
Robot Arm 1 togoal: 14.0923532011177
Mobile Platform 2 Pose: [-2.34140795956059;3.41605010252233;-9.88656548428639]
Robot Arm 2 end: [2.68031176383509;-9.8672324756791;0.575044946472278]
Robot Arm 1 togoal: 12.7162401914585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798238e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.792539e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.785100564480858;-3.66185100762296;10.482170696198]
Robot Arm 1 end: [-2.98342505025892;10.4837395891775;0.475296231648683]
Robot Arm 1 togoal: 13.0344476828507
Mobile Platform 2 Pose: [-2.34274475902677;3.31497638434666;-9.86585585550771]
Robot Arm 2 end: [3.18195271451646;-9.85692245215468;1.44108668210585]
Robot Arm 1 togoal: 13.1829713908065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796169e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.78664656753193;-3.5809306825849;10.2953365010935]
Robot Arm 1 end: [-3.32977475974413;10.3011407563115;1.37923178188198]
Robot Arm 1 togoal: 13.3339296206442
Mobile Platform 2 Pose: [-2.34274239639812;3.28390518494065;-9.49401182415722]
Robot Arm 2 end: [3.97156127970018;-9.49755445952178;0.906012251497383]
Robot Arm 1 togoal: 13.9941125493678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.791210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.795696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.78664411762449;-3.54940077233446;10.7677517463206]
Robot Arm 1 end: [-4.14030961233631;10.7828384813102;1.06078642795043]
Robot Arm 1 togoal: 14.1694385404803
Mobile Platform 2 Pose: [-2.34406201785695;3.18263805074082;-9.57628105311713]
Robot Arm 2 end: [3.34093007753866;-9.56581296241756;0.0258087454565408]
Robot Arm 1 togoal: 13.4314753696325

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.793767e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.790316350226966;-3.44469167345037;10.801307747206]
Robot Arm 1 end: [-3.8509229136952;10.8191116545411;0.181039771912092]
Robot Arm 1 togoal: 13.9396737005064
Mobile Platform 2 Pose: [-2.34406361805958;3.15422897924094;-10.0083473361559]
Robot Arm 2 end: [2.45286236631589;-9.98680626068785;0.840126643370736]
Robot Arm 1 togoal: 12.4714691883888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804705e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.795521e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.790315327637617;-3.41516095881545;10.4638831263347]
Robot Arm 1 end: [-2.78111729186289;10.4674148161276;0.486969748451247]
Robot Arm 1 togoal: 12.8313933148279
Mobile Platform 2 Pose: [-2.34406581800624;3.12339869517583;-9.66861965369644]
Robot Arm 2 end: [3.1696153474856;-9.66236095871161;1.42373029474725]
Robot Arm 1 togoal: 13.1743018759436

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800663e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799678e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.79234411236699;-3.36466551389397;10.2200056909426]
Robot Arm 1 end: [-3.08507302710725;10.2260444379403;1.33829184244592]
Robot Arm 1 togoal: 13.0883006721075
Mobile Platform 2 Pose: [-2.34473241509654;3.04226236186046;-9.48197423803895]
Robot Arm 2 end: [3.72369188295717;-9.48497877719291;0.660278575351548]
Robot Arm 1 togoal: 13.7602982209375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800127e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.792340650072957;-3.33371816319183;10.7047028042871]
Robot Arm 1 end: [-3.94659398901814;10.7216426485183;0.966861319900662]
Robot Arm 1 togoal: 13.9762413288931
Mobile Platform 2 Pose: [-2.34539653279998;2.95605678328275;-9.65626675472254]
Robot Arm 2 end: [2.69367237805387;-9.64010166591582;0.0839862162186193]
Robot Arm 1 togoal: 12.7798221375347

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.795616e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807381e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.796436277176444;-3.22692301820172;10.787885861305]
Robot Arm 1 end: [-3.63901423679612;10.8079547417323;0.204386789802714]
Robot Arm 1 togoal: 13.7262147207991
Mobile Platform 2 Pose: [-2.34540145818741;2.92750752018561;-10.1379843290673]
Robot Arm 2 end: [2.60453667821249;-10.1230513283552;1.32848452691047]
Robot Arm 1 togoal: 12.6066073433415

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805564e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800643e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.796435334499433;-3.19779692211914;10.4529943679731]
Robot Arm 1 end: [-2.56157344116053;10.456787365784;0.842525871723753]
Robot Arm 1 togoal: 12.5881733844887
Mobile Platform 2 Pose: [-2.34539845856237;2.89680272705146;-9.59462718739213]
Robot Arm 2 end: [3.51904259538743;-9.59469383417064;0.995404574169155]
Robot Arm 1 togoal: 13.5353254979558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796334e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801012e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.796436260626337;-3.14610621406904;10.6981485422977]
Robot Arm 1 end: [-3.17493931167297;10.705675721262;1.38164058641059]
Robot Arm 1 togoal: 13.1945604373924
Mobile Platform 2 Pose: [-2.34826506604694;2.7950756384613;-9.61912091001523]
Robot Arm 2 end: [2.93020186132487;-9.60724183985578;0.0533316445173729]
Robot Arm 1 togoal: 13.0191562533312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.796419550115117;-3.08976141940882;10.9310471361556]
Robot Arm 1 end: [-3.63051746769797;10.9476959637905;0.360969127239199]
Robot Arm 1 togoal: 13.7125783754465
Mobile Platform 2 Pose: [-2.34826705421376;2.7670588084511;-10.0626525463119]
Robot Arm 2 end: [2.3832342504345;-10.0453640479128;1.28120386123702]
Robot Arm 1 togoal: 12.3856388848595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801275e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801048e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.796418049706606;-3.05956366521503;10.5542695827897]
Robot Arm 1 end: [-2.43177841669847;10.5586166882355;0.498868355672319]
Robot Arm 1 togoal: 12.4862292268163
Mobile Platform 2 Pose: [-2.34826579596973;2.73661481852995;-9.46664088995385]
Robot Arm 2 end: [3.4000349474661;-9.463084936718;0.852577746081423]
Robot Arm 1 togoal: 13.4274347024302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804601e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.796415963677089;-2.99676938219667;10.335694274906]
Robot Arm 1 end: [-3.3888932358046;10.3449065991666;1.23466070806924]
Robot Arm 1 togoal: 13.3963955164076
Mobile Platform 2 Pose: [-2.34894793581756;2.65391604009148;-9.64666457219952]
Robot Arm 2 end: [2.20756193143805;-9.62809523758372;0.213331623269884]
Robot Arm 1 togoal: 12.2830321193793

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.796412856963039;-2.96548771349487;10.7993887942202]
Robot Arm 1 end: [-3.26925340755908;10.8148887598351;0.142330139557972]
Robot Arm 1 togoal: 13.3655475110081
Mobile Platform 2 Pose: [-2.34895535049279;2.6248826772594;-10.1532646483899]
Robot Arm 2 end: [2.76183156739013;-10.151255133995;1.3824586622591]
Robot Arm 1 togoal: 12.7634798065519

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806872e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803943e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.79641491925492;-2.93706780795568;10.2255776872648]
Robot Arm 1 end: [-2.67427169860015;10.2336560123881;1.30411406433911]
Robot Arm 1 togoal: 12.6782805510507
Mobile Platform 2 Pose: [-2.34895184242342;2.59547100073244;-9.6175825966394]
Robot Arm 2 end: [2.82097312615027;-9.61141843155156;0.0870066213423815]
Robot Arm 1 togoal: 12.9067689429554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807415e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.796412980988606;-2.90574389379228;10.8187371398591]
Robot Arm 1 end: [-3.54647762573508;10.8374386844597;0.626730329108744]
Robot Arm 1 togoal: 13.6017674534983
Mobile Platform 2 Pose: [-2.34895557219448;2.56729218014764;-10.270082081912]
Robot Arm 2 end: [2.29688584521243;-10.2567496572166;1.33872377343391]
Robot Arm 1 togoal: 12.3009164902229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806156e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.796412608555872;-2.87715396191069;10.4519500824721]
Robot Arm 1 end: [-2.27091229688486;10.4572529154982;0.910844787561571]
Robot Arm 1 togoal: 12.2945784071613
Mobile Platform 2 Pose: [-2.34895225364519;2.53795027717728;-9.70115283549683]
Robot Arm 2 end: [3.01315696808959;-9.70059112800109;0.248310722709239]
Robot Arm 1 togoal: 13.0786711077732

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797116e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.796413370779755;-2.8456741145522;10.8132577163649]
Robot Arm 1 end: [-3.37867341084656;10.8295326037401;1.08686798091771]
Robot Arm 1 togoal: 13.4113943042755
Mobile Platform 2 Pose: [-2.34895475782986;2.50992921032193;-10.2565797934293]
Robot Arm 2 end: [2.09608697585461;-10.2390280422138;1.25106175299234]
Robot Arm 1 togoal: 12.1014594652712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805161e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.796385949524638;-2.74945949523764;10.9340167147154]
Robot Arm 1 end: [-2.16293959477581;10.9391966890638;0.481995180306221]
Robot Arm 1 togoal: 12.2433133187982
Mobile Platform 2 Pose: [-2.3489517063022;2.48073432506483;-9.70656749925477]
Robot Arm 2 end: [2.98203138393615;-9.7050352071792;0.27972159550514]
Robot Arm 1 togoal: 13.0445741655285

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807520e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [0.796389217768113;-2.71850655694625;10.4668072294755]
Robot Arm 1 end: [-3.088277442772;10.4776188719654;1.25076944933771]
Robot Arm 1 togoal: 13.0997767445229
Mobile Platform 2 Pose: [-2.34895484685904;2.45273575890472;-10.2623839938476]
Robot Arm 2 end: [2.08381370109663;-10.2460138173243;1.28056086332003]
Robot Arm 1 togoal: 12.0887090849095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806462e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805285e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [0.796386960693799;-2.69016577472823;10.9626187506891]
Robot Arm 1 end: [-2.11529028215587;10.9682447805179;0.459867600954414]
Robot Arm 1 togoal: 12.2001540377245
Mobile Platform 2 Pose: [-2.3489516650119;2.4239581638088;-9.70587564438954]
Robot Arm 2 end: [2.73927376969236;-9.7008460226713;0.134842125717604]
Robot Arm 1 togoal: 12.8179570499264

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806284e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [0.796388887869838;-2.65914361815588;10.4065358246199]
Robot Arm 1 end: [-3.06466916595536;10.4183593077016;1.2208504807328]
Robot Arm 1 togoal: 13.0748114579187
Mobile Platform 2 Pose: [-2.34895645282158;2.39559602176526;-10.3638453291001]
Robot Arm 2 end: [2.24510521755977;-10.3546061217443;1.37124893573317]
Robot Arm 1 togoal: 12.2511539322603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807219e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806593e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [0.79638693508099;-2.6311368443192;10.9378049469799]
Robot Arm 1 end: [-2.00569825873209;10.9429380566414;0.65347946915363]
Robot Arm 1 togoal: 12.0738773693385
Mobile Platform 2 Pose: [-2.34895287702953;2.36733017872682;-9.79619719248777]
Robot Arm 2 end: [2.25906220663535;-9.78469911274563;0.066614198018535]
Robot Arm 1 togoal: 12.3469104928809

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798677e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [0.796387119756598;-2.60031267324296;10.4976225993745]
Robot Arm 1 end: [-3.19588716260179;10.5158861746206;0.954611993754805]
Robot Arm 1 togoal: 13.2181077286767
Mobile Platform 2 Pose: [-2.34895677794305;2.33806089979544;-10.4406590327897]
Robot Arm 2 end: [2.49528202383142;-10.4414786168432;1.36626843551703]
Robot Arm 1 togoal: 12.5040360716555

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808413e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [0.796387195908681;-2.57279721286511;10.9046108818551]
Robot Arm 1 end: [-2.09134073990305;10.911567120979;1.12454984083636]
Robot Arm 1 togoal: 12.132132880627
Mobile Platform 2 Pose: [-2.34895321315301;2.31017634640471;-9.86268324690852]
Robot Arm 2 end: [1.79893336734034;-9.842552099383;0.302557683189109]
Robot Arm 1 togoal: 11.8627239758725

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801205e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [0.796360384749758;-2.4732012743797;10.8021447400065]
Robot Arm 1 end: [-2.88231232918856;10.8220433320341;0.228998396592852]
Robot Arm 1 togoal: 12.973010226224
Mobile Platform 2 Pose: [-2.34895414516864;2.28048071705914;-10.3546893050035]
Robot Arm 2 end: [2.81108134026915;-10.3602583586521;1.11440150901958]
Robot Arm 1 togoal: 12.8225938688254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.809250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [0.796365704261358;-2.44521967080567;10.277064394748]
Robot Arm 1 end: [-2.64132465140737;10.2840602360705;1.32050481569096]
Robot Arm 1 togoal: 12.6461052296321
Mobile Platform 2 Pose: [-2.34895219202533;2.25327272483702;-9.80608879419387]
Robot Arm 2 end: [1.6367654150928;-9.78400240146508;0.951173121663583]
Robot Arm 1 togoal: 11.652710712952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [0.796362431775545;-2.41648153294449;10.7818208999029]
Robot Arm 1 end: [-1.87573655551792;10.7880603190238;0.365523400269573]
Robot Arm 1 togoal: 11.9578126751931
Mobile Platform 2 Pose: [-2.34572984700025;2.15913862847384;-9.95769076347605]
Robot Arm 2 end: [2.58034233088482;-9.95016304294461;0.214411665207407]
Robot Arm 1 togoal: 12.6481093433429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803510e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807334e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [0.79636201907042;-2.38729198865141;10.2848199571303]
Robot Arm 1 end: [-3.01489992155454;10.3029721508265;0.652051652445989]
Robot Arm 1 togoal: 13.0473937367956
Mobile Platform 2 Pose: [-2.34573586612339;2.13130058801608;-10.3624735984822]
Robot Arm 2 end: [2.25109845027385;-10.3606558902285;1.36625710408651]
Robot Arm 1 togoal: 12.2573827252515

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [0.796363733448436;-2.35855690629251;10.5900644009367]
Robot Arm 1 end: [-1.94894832344681;10.5969285216997;1.21768367352148]
Robot Arm 1 togoal: 11.9676936161622
Mobile Platform 2 Pose: [-2.34573198627774;2.10411684251819;-9.82310087936158]
Robot Arm 2 end: [1.46704432732789;-9.80068979123051;0.54726505802702]
Robot Arm 1 togoal: 11.5100386576867

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799980e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805623e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [0.796357812420951;-2.28822119095763;10.7867818466282]
Robot Arm 1 end: [-1.68001311501194;10.7920113709484;0.590612311995992]
Robot Arm 1 togoal: 11.7437476824288
Mobile Platform 2 Pose: [-2.34573003966164;2.07570794369373;-10.1600728321001]
Robot Arm 2 end: [2.64671446344291;-10.1550242707874;0.402225734786785]
Robot Arm 1 togoal: 12.6970498581715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804116e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805268e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [0.796356367388884;-2.25950954839825;10.5014800641272]
Robot Arm 1 end: [-2.760170735267;10.5249432197912;0.333796068873258]
Robot Arm 1 togoal: 12.8260272785946
Mobile Platform 2 Pose: [-2.34312021134928;1.9826485862186;-10.2867644320859]
Robot Arm 2 end: [1.97877146396648;-10.2807143076089;1.37732323896189]
Robot Arm 1 togoal: 11.9829215686366

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [0.796362752942737;-2.23130119308489;10.1167150897271]
Robot Arm 1 end: [-2.40996246801815;10.1232664666909;1.31996845982593]
Robot Arm 1 togoal: 12.4122027360003
Mobile Platform 2 Pose: [-2.34311672512026;1.95657904794445;-9.83262370110716]
Robot Arm 2 end: [1.30891117565911;-9.81000215743718;0.785174676569406]
Robot Arm 1 togoal: 11.3344170589284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [0.796358999907973;-2.20318389305395;10.6338798624022]
Robot Arm 1 end: [-1.56680409343439;10.6391166938239;0.637971042856273]
Robot Arm 1 togoal: 11.6180535040672
Mobile Platform 2 Pose: [-2.3425124743833;1.88008683497899;-10.0188105500587]
Robot Arm 2 end: [1.87496146233889;-10.0052889711562;0.066642135051]
Robot Arm 1 togoal: 11.9636906723882

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806805e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805108e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [0.796356371917479;-2.17532731734771;10.3302306515441]
Robot Arm 1 end: [-2.36077194518959;10.348238387243;0.126077679832246]
Robot Arm 1 togoal: 12.4441054855155
Mobile Platform 2 Pose: [-2.3425139557468;1.85155566222972;-10.3288779813187]
Robot Arm 2 end: [2.36214998443312;-10.3259037913894;1.12235092507853]
Robot Arm 1 togoal: 12.372868972223

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [0.796360412490214;-2.14567017776277;9.98945518727303]
Robot Arm 1 end: [-2.39442461351788;10.0002322754751;1.3014190168371]
Robot Arm 1 togoal: 12.3963695234652
Mobile Platform 2 Pose: [-2.34251450293852;1.82499474990429;-9.8090682479983]
Robot Arm 2 end: [1.42222734366394;-9.79113171513018;1.23638345829048]
Robot Arm 1 togoal: 11.4276817433189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811519e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797738e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [0.796358839376963;-2.11961199003732;10.5690009796913]
Robot Arm 1 end: [-1.76205207107705;10.5773417127294;1.22502273445274]
Robot Arm 1 togoal: 11.7799318723712
Mobile Platform 2 Pose: [-2.34372805447605;1.73888701480795;-9.74371776932154]
Robot Arm 2 end: [1.22360791162282;-9.72361789973864;1.11120137811024]
Robot Arm 1 togoal: 11.2344869277457

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.788462e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796702e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [0.797681269164126;-2.02696822232453;10.4188136703585]
Robot Arm 1 end: [-1.44880232243056;10.4247910148479;0.989899165215822]
Robot Arm 1 togoal: 11.4689838312335
Mobile Platform 2 Pose: [-2.34569105799275;1.65223256149476;-9.70395292962152]
Robot Arm 2 end: [1.03144162336336;-9.68116383280478;0.923480013063164]
Robot Arm 1 togoal: 11.0522120376861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804677e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [0.797682763413527;-2.00187718715119;10.7110622065755]
Robot Arm 1 end: [-1.39805267728504;10.7169883943519;0.837552888338688]
Robot Arm 1 togoal: 11.4410129423158
Mobile Platform 2 Pose: [-2.34831840892712;1.5686989185114;-9.73579325198323]
Robot Arm 2 end: [0.935937418284019;-9.71342153017025;0.839100743401562]
Robot Arm 1 togoal: 10.9609233658549

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800683e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [0.797683181833973;-1.97466892561893;10.409002700663]
Robot Arm 1 end: [-1.81000343867414;10.4183958890317;0.110741752822097]
Robot Arm 1 togoal: 11.9012631541861
Mobile Platform 2 Pose: [-2.35150992050592;1.48381684430658;-9.84680198267861]
Robot Arm 2 end: [0.837892424579123;-9.82356434638885;0.639145276870865]
Robot Arm 1 togoal: 10.875141815136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.787501e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [0.800182465148044;-1.88024457682241;10.3011188351885]
Robot Arm 1 end: [-2.04648662471254;10.3166120687322;0.126259446126529]
Robot Arm 1 togoal: 12.1310916414832
Mobile Platform 2 Pose: [-2.35517450872171;1.4012328066674;-9.76472642399541]
Robot Arm 2 end: [0.807070158450344;-9.74313701626884;0.479635311747389]
Robot Arm 1 togoal: 10.860165027108

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.790920e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801199e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [0.803087166623928;-1.78409671620381;10.1721368305894]
Robot Arm 1 end: [-2.40014604097225;10.1975162553906;0.633181953918429]
Robot Arm 1 togoal: 12.4334570973455
Mobile Platform 2 Pose: [-2.35714152176086;1.34836166880771;-9.98759345862334]
Robot Arm 2 end: [0.835153757031423;-9.96737603634964;0.324255424645237]
Robot Arm 1 togoal: 10.9010925525911

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.788492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804665e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [0.806216445937381;-1.69188978529315;10.1267163592401]
Robot Arm 1 end: [-2.3088210971707;10.134835219628;0.76417451740774]
Robot Arm 1 togoal: 12.332803499193
Mobile Platform 2 Pose: [-2.35920943318668;1.28151319595969;-9.79106075651899]
Robot Arm 2 end: [0.886806102095509;-9.77197664610546;0.218083345747341]
Robot Arm 1 togoal: 10.966865256876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803321e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812601e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [0.806212584757343;-1.66300381146014;10.4910196127729]
Robot Arm 1 end: [-1.35739054566725;10.4997412449234;1.26999309190512]
Robot Arm 1 togoal: 11.371150592029
Mobile Platform 2 Pose: [-2.35920751920232;1.25586463992784;-10.0860462838764]
Robot Arm 2 end: [1.57162782854214;-10.0684160449367;0.161430271448483]
Robot Arm 1 togoal: 11.6514239989921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802447e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [0.806477405700025;-1.63770733944845;10.7673522823077]
Robot Arm 1 end: [-1.23802757033272;10.7756102019864;1.18412202634321]
Robot Arm 1 togoal: 11.269796875992
Mobile Platform 2 Pose: [-2.36131540487674;1.19756568997056;-10.3048790076727]
Robot Arm 2 end: [1.8247955551503;-10.2987189950305;0.841434766400988]
Robot Arm 1 togoal: 11.8480729178129

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [0.808412705655604;-1.57886054654783;10.5518269956303]
Robot Arm 1 end: [-1.02705479097285;10.5584219737857;1.0217806172585]
Robot Arm 1 togoal: 11.0524654469502
Mobile Platform 2 Pose: [-2.36131744213122;1.17110307442559;-9.8928945753576]
Robot Arm 2 end: [1.14798550967574;-9.88148271545925;1.36291827569736]
Robot Arm 1 togoal: 11.1497361898044

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810650e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810627e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [0.810435911078864;-1.54130851675575;10.802109627465]
Robot Arm 1 end: [-0.970937603804534;10.8085004275831;0.934944860897313]
Robot Arm 1 togoal: 11.0162881894966
Mobile Platform 2 Pose: [-2.36320922669018;1.106010050826;-9.69501772054321]
Robot Arm 2 end: [0.988333816535085;-9.68201044150689;1.34543030802493]
Robot Arm 1 togoal: 10.9943359117165

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800282e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [0.810437756755827;-1.51524599054748;10.4661563056008]
Robot Arm 1 end: [-1.00361887012509;10.4730883637162;0.35952273495715]
Robot Arm 1 togoal: 11.0748575736245
Mobile Platform 2 Pose: [-2.36656931859758;1.02083366072451;-9.75975755686461]
Robot Arm 2 end: [0.75561179024586;-9.74574382168695;1.30520500316519]
Robot Arm 1 togoal: 10.7607805789053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803016e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810607e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [0.814097304021776;-1.42918961847732;10.6220070162704]
Robot Arm 1 end: [-1.019396609325;10.6300712683926;0.27144627743777]
Robot Arm 1 togoal: 11.1079015093953
Mobile Platform 2 Pose: [-2.36839884271525;0.942208998256588;-9.58808977826803]
Robot Arm 2 end: [0.606177413896563;-9.57135846019681;1.25698257312935]
Robot Arm 1 togoal: 10.6181183882261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [0.814095736048446;-1.40299948102827;10.2984493895199]
Robot Arm 1 end: [-1.55237858458017;10.3169713435916;0.120182295418042]
Robot Arm 1 togoal: 11.6413148757172
Mobile Platform 2 Pose: [-2.36840475913976;0.917829316508082;-9.87769129068936]
Robot Arm 2 end: [0.413295584523085;-9.85715524195182;1.1162521346978]
Robot Arm 1 togoal: 10.4221375154052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.793678e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817441e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [0.817246478681586;-1.31238212315346;10.2124134338474]
Robot Arm 1 end: [-1.591100259143;10.233415346392;0.178836359721945]
Robot Arm 1 togoal: 11.6708821936784
Mobile Platform 2 Pose: [-2.36840250749356;0.894360928558238;-9.6025520104761]
Robot Arm 2 end: [0.321225571687779;-9.58047857457022;0.981660931336146]
Robot Arm 1 togoal: 10.3438185482463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.792293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [0.819929973081756;-1.22097428854486;10.3110007394095]
Robot Arm 1 end: [-1.67566973015366;10.3395228061894;0.307860585273539]
Robot Arm 1 togoal: 11.7434341920544
Mobile Platform 2 Pose: [-2.36840381500906;0.870136865380666;-9.94045786149471]
Robot Arm 2 end: [0.23965076621666;-9.91665659585258;0.610999543256381]
Robot Arm 1 togoal: 10.2803450704911

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816710e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [0.819931781660307;-1.13768488348424;10.1395074759235]
Robot Arm 1 end: [-1.66421824042321;10.1672293706528;0.415303544425102]
Robot Arm 1 togoal: 11.7177010319645
Mobile Platform 2 Pose: [-2.36899007668046;0.840970509158605;-9.67732693370513]
Robot Arm 2 end: [0.263766424685934;-9.65558500418651;0.475261453298087]
Robot Arm 1 togoal: 10.3226494669687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806987e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [0.819932006304324;-1.1102277127961;10.442481394152]
Robot Arm 1 end: [-1.50825762421312;10.4667554367389;1.1871454457402]
Robot Arm 1 togoal: 11.5225567061475
Mobile Platform 2 Pose: [-2.36898919938026;0.81669490656308;-10.0140835599982]
Robot Arm 2 end: [0.471439674584447;-9.99608677974773;0.185575863185042]
Robot Arm 1 togoal: 10.5562503663442

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.809527e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [0.819953767127018;-1.02747699871845;10.5255675308412]
Robot Arm 1 end: [-1.34372771598796;10.5342509858488;1.23928703119631]
Robot Arm 1 togoal: 11.3597951041005
Mobile Platform 2 Pose: [-2.36903594600243;0.764540021344268;-9.79118887560222]
Robot Arm 2 end: [0.558444945977824;-9.77308048257744;0.129715082015328]
Robot Arm 1 togoal: 10.6521324563509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.801163e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [0.819950165002236;-1.00144723462825;10.8106082372635]
Robot Arm 1 end: [-1.05353888648529;10.8238274390856;1.33267105552866]
Robot Arm 1 togoal: 11.0857963789598
Mobile Platform 2 Pose: [-2.36903380472122;0.739524122748769;-10.1144428782278]
Robot Arm 2 end: [1.03906801513594;-10.0950946871286;0.165207088167813]
Robot Arm 1 togoal: 11.1224206026427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799552e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [0.819957145182775;-0.976662044010321;10.4740143660588]
Robot Arm 1 end: [-0.827164652480562;10.4786311051539;1.30611216203372]
Robot Arm 1 togoal: 10.8398689535276
Mobile Platform 2 Pose: [-2.36814006700529;0.653234312374969;-10.1078437604646]
Robot Arm 2 end: [1.06181586106827;-10.0931050718213;0.246964114713553]
Robot Arm 1 togoal: 11.1353314335757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812645e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798869e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [0.819950762094386;-0.951366983690582;10.8078969384603]
Robot Arm 1 end: [-0.488238911240644;10.8151194151688;1.12679388775068]
Robot Arm 1 togoal: 10.5272490983724
Mobile Platform 2 Pose: [-2.36676093989935;0.566514296288983;-9.96700763804051]
Robot Arm 2 end: [1.10291526884265;-9.94966562290303;0.394453582887227]
Robot Arm 1 togoal: 11.1600344016731

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817349e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.803914e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [0.819955123438743;-0.927732873411757;10.4456420459354]
Robot Arm 1 end: [-0.504898558645854;10.4526646890651;1.14891399862084]
Robot Arm 1 togoal: 10.5212283997919
Mobile Platform 2 Pose: [-2.36515062331931;0.482222707286375;-10.1297991423812]
Robot Arm 2 end: [1.07796200769611;-10.1181012932086;0.533851912301843]
Robot Arm 1 togoal: 11.1224840528752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812583e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [0.819952021765393;-0.902883758604146;10.7584221984475]
Robot Arm 1 end: [-0.344290615058842;10.764978108396;0.977354767658394]
Robot Arm 1 togoal: 10.3867747628302
Mobile Platform 2 Pose: [-2.36515135310894;0.456284587742509;-9.81115783688679]
Robot Arm 2 end: [0.884195013537712;-9.80025376993689;1.17908019741806]
Robot Arm 1 togoal: 10.8913960905176

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817756e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817974e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [0.819953678599268;-0.87941002745587;10.4084971088726]
Robot Arm 1 end: [-0.371094253999549;10.4156323466116;1.03932513647604]
Robot Arm 1 togoal: 10.3905898267155
Mobile Platform 2 Pose: [-2.36236877253638;0.377523016515104;-9.87943057362348]
Robot Arm 2 end: [0.687463410040811;-9.87967301243393;1.26018232714808]
Robot Arm 1 togoal: 10.6913225834491

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [0.819953132698863;-0.854858356980842;10.6921858726185]
Robot Arm 1 end: [-0.258636298589675;10.6988471444525;0.864093249799627]
Robot Arm 1 togoal: 10.3033748119978
Mobile Platform 2 Pose: [-2.36236533484531;0.352737384594679;-9.55064140798271]
Robot Arm 2 end: [0.237153866281369;-9.53680763114534;1.33869888112539]
Robot Arm 1 togoal: 10.2492487602959

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818115e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [0.819952522488716;-0.831524867745372;10.3694829111332]
Robot Arm 1 end: [-0.284022326194099;10.376379413944;0.961781190384854]
Robot Arm 1 togoal: 10.3060905461735
Mobile Platform 2 Pose: [-2.36237088925469;0.330123041522247;-9.86714095433619]
Robot Arm 2 end: [0.301123631065214;-9.86135850077627;1.3393579898634]
Robot Arm 1 togoal: 10.3036577654561

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [0.819955464481566;-0.789931292329038;10.6117715489934]
Robot Arm 1 end: [-0.187007197860249;10.6186502980233;0.823377849076972]
Robot Arm 1 togoal: 10.229590436863
Mobile Platform 2 Pose: [-2.36236592939654;0.305644507449812;-9.52313711999083]
Robot Arm 2 end: [-0.0679085365377896;-9.50522389537406;1.22617870480747]
Robot Arm 1 togoal: 9.94877700164129

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818598e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [0.819953685761386;-0.766782344643466;10.324227715183]
Robot Arm 1 end: [-0.219699624322686;10.3310787734078;0.959015555149211]
Robot Arm 1 togoal: 10.2404931561833
Mobile Platform 2 Pose: [-2.36237055505706;0.283479443978002;-9.87931668025592]
Robot Arm 2 end: [0.0801908821442308;-9.86778510472666;1.30303662238787]
Robot Arm 1 togoal: 10.0834139572357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816175e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [0.81995895207617;-0.713266594882158;10.5440208906153]
Robot Arm 1 end: [-0.116374506473803;10.5510969981359;0.846288358503551]
Robot Arm 1 togoal: 10.1538158765933
Mobile Platform 2 Pose: [-2.36236660776974;0.259321683321915;-9.54221441736866]
Robot Arm 2 end: [-0.195196467357208;-9.52286690164923;1.15316635626976]
Robot Arm 1 togoal: 9.82329526669774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.822362e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [0.819953639508969;-0.674082679720885;10.3012156715611]
Robot Arm 1 end: [-0.156232800921661;10.3082692209202;1.01453065246715]
Robot Arm 1 togoal: 10.1735247490275
Mobile Platform 2 Pose: [-2.3623698781855;0.237343439519171;-9.89759686062395]
Robot Arm 2 end: [-0.025264992022826;-9.88381413019108;1.27628831095382]
Robot Arm 1 togoal: 9.97841277190238

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [0.81996071489078;-0.606542580513658;10.4946525575683]
Robot Arm 1 end: [-0.0345586799328255;10.5018938959756;0.927539896438614]
Robot Arm 1 togoal: 10.0646142637635
Mobile Platform 2 Pose: [-2.36236721609288;0.213362608551415;-9.57310292257561]
Robot Arm 2 end: [-0.250304480376466;-9.55360849066306;1.14116512889138]
Robot Arm 1 togoal: 9.76729761163677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819757e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.822677e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [0.819953945570039;-0.536918001463146;10.3090639732107]
Robot Arm 1 end: [-0.0961377992433154;10.3166180004181;1.11930853019749]
Robot Arm 1 togoal: 10.1090850949238
Mobile Platform 2 Pose: [-2.36236914222843;0.191498610482713;-9.91688123135748]
Robot Arm 2 end: [-0.0605165697301614;-9.90311001354387;1.27892624832009]
Robot Arm 1 togoal: 9.9429029076692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815956e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [0.81996797167732;-0.456789257707028;10.442544187436]
Robot Arm 1 end: [0.0494930779513761;10.449661562321;1.05425393305663]
Robot Arm 1 togoal: 9.97159141849352
Mobile Platform 2 Pose: [-2.36236759807386;0.167550928267778;-9.6062757198505]
Robot Arm 2 end: [-0.275893329157911;-9.58733073394722;1.16024297998252]
Robot Arm 1 togoal: 9.73954270469369

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820426e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.822953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [0.819953935789764;-0.381915315468696;10.3800828176609]
Robot Arm 1 end: [-0.0877093090446082;10.3885602417959;1.23608353784418]
Robot Arm 1 togoal: 10.0992096500517
Mobile Platform 2 Pose: [-2.36236827974701;0.145784919225603;-9.93499213770661]
Robot Arm 2 end: [-0.060406601566025;-9.92223000163941;1.29542724931173]
Robot Arm 1 togoal: 9.94245684751462

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815581e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [0.819971976661709;-0.301293263472195;10.3295081074895]
Robot Arm 1 end: [0.101335318403878;10.3357783136191;1.1710954592809]
Robot Arm 1 togoal: 9.91053695529113
Mobile Platform 2 Pose: [-2.36236790440451;0.12178940692597;-9.63677702689344]
Robot Arm 2 end: [-0.287251279798255;-9.61886724991391;1.19034972584111]
Robot Arm 1 togoal: 9.72584595183217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820944e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823217e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [0.819959391436101;-0.24667082386966;10.5416948889133]
Robot Arm 1 end: [-0.129265696050637;10.5516894975902;1.30166552709994]
Robot Arm 1 togoal: 10.146649321522
Mobile Platform 2 Pose: [-2.36236739590301;0.100116393604604;-9.95009648673407]
Robot Arm 2 end: [-0.0433463287105217;-9.93869710028185;1.31238649732478]
Robot Arm 1 togoal: 9.95902756141454

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814806e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814629e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [0.819966157753667;-0.22218503143026;10.2341411406843]
Robot Arm 1 end: [0.0763374874837193;10.239014684205;1.24344046706728]
Robot Arm 1 togoal: 9.93042018123646
Mobile Platform 2 Pose: [-2.36236836817099;0.0760502969344179;-9.66277335303195]
Robot Arm 2 end: [-0.281991280985912;-9.64653470062096;1.2283099348559]
Robot Arm 1 togoal: 9.72883850492612

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820748e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823394e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [0.819958714974726;-0.199839317929552;10.5810286882511]
Robot Arm 1 end: [-0.188190782899679;10.5926988287303;1.31232178280009]
Robot Arm 1 togoal: 10.2075496535003
Mobile Platform 2 Pose: [-2.36236670620974;0.0544366563243819;-9.96071391800936]
Robot Arm 2 end: [-0.0146820419760369;-9.95081877067016;1.32392695116933]
Robot Arm 1 togoal: 9.98738360646719

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814295e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.813004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [0.819966922553426;-0.175156070461322;10.2230319356112]
Robot Arm 1 end: [0.0636883288567759;10.2267128757778;1.27123103508101]
Robot Arm 1 togoal: 9.94203565121234
Mobile Platform 2 Pose: [-2.36236955346156;0.0303495973938148;-9.6833269299598]
Robot Arm 2 end: [-0.238126236891104;-9.67015133337912;1.27879875869645]
Robot Arm 1 togoal: 9.77044732604574

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820788e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823233e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [0.819957927134404;-0.152825360632037;10.5983131031526]
Robot Arm 1 end: [-0.206732503457593;10.6117748955553;1.3088901992876]
Robot Arm 1 togoal: 10.2272502407797
Mobile Platform 2 Pose: [-2.36236678591017;0.00866920326311549;-9.96460418954214]
Robot Arm 2 end: [0.0264770612429956;-9.95630329255099;1.32608928919645]
Robot Arm 1 togoal: 10.0284665750582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812738e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [0.81996753067655;-0.128097992377623;10.2233231366424]
Robot Arm 1 end: [0.0568130815177626;10.2257810066487;1.2896932633877]
Robot Arm 1 togoal: 9.94843937335107
Mobile Platform 2 Pose: [-2.36352018550193;-0.0241326631235357;-9.70640044329186]
Robot Arm 2 end: [-0.230657696453492;-9.69562018956041;1.30279991552492]
Robot Arm 1 togoal: 9.77651816291115

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820729e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823497e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [0.819957033106781;-0.105745690444089;10.6190391076684]
Robot Arm 1 end: [-0.208651305260356;10.634525320362;1.30119501385339]
Robot Arm 1 togoal: 10.2307134690846
Mobile Platform 2 Pose: [-2.36499491426818;-0.0582727600275525;-9.95468124206347]
Robot Arm 2 end: [0.0435333334858394;-9.9474655652463;1.31624193862487]
Robot Arm 1 togoal: 10.0457576892624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814220e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [0.819968076989743;-0.0810269215901247;10.229241691841]
Robot Arm 1 end: [0.0533074435814327;10.2304113519848;1.30186429121578]
Robot Arm 1 togoal: 9.95177384499368
Mobile Platform 2 Pose: [-2.36673294701654;-0.108068575309881;-9.72787545447993]
Robot Arm 2 end: [-0.195880849370088;-9.72147211666133;1.3299304984314]
Robot Arm 1 togoal: 9.80993566013432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [0.819956039535637;-0.0586350517622733;10.6460070570252]
Robot Arm 1 end: [-0.201405541194213;10.6637826871578;1.29154286611085]
Robot Arm 1 togoal: 10.2255529458299
Mobile Platform 2 Pose: [-2.36867465436033;-0.175685427113782;-9.91416262537373]
Robot Arm 2 end: [0.0284966883053404;-9.9080084830849;1.28797897066945]
Robot Arm 1 togoal: 10.0316253452093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814316e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [0.819968619032469;-0.0339490149866094;10.2396281206806]
Robot Arm 1 end: [0.0523296315949942;10.2394076851637;1.30911964466338]
Robot Arm 1 togoal: 9.95280639232654
Mobile Platform 2 Pose: [-2.37268971370769;-0.256486192821151;-9.75387539762575]
Robot Arm 2 end: [-0.213356171734412;-9.75221959744114;1.33240939581189]
Robot Arm 1 togoal: 9.79159634407021

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820476e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823598e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [0.819954945680536;-0.0115041891352693;10.6788966179063]
Robot Arm 1 end: [-0.187521241469305;10.6992300808179;1.28100586337803]
Robot Arm 1 togoal: 10.2143090288537
Mobile Platform 2 Pose: [-2.37698121741228;-0.328385130988191;-9.814516459699]
Robot Arm 2 end: [0.00433509786086184;-9.80972463219038;1.22226921961203]
Robot Arm 1 togoal: 10.0106026564357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814453e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [0.819969196989675;0.0131342702210787;10.2531026841641]
Robot Arm 1 end: [0.0538345615031927;10.2513507101976;1.31226107474589]
Robot Arm 1 togoal: 9.95153033700754
Mobile Platform 2 Pose: [-2.3812478409601;-0.409157771802252;-9.83120448546735]
Robot Arm 2 end: [-0.21519436089772;-9.83370208942548;1.29972269723029]
Robot Arm 1 togoal: 9.78872004067898

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823590e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [0.81995375090576;0.035643862519912;10.7167094848708]
Robot Arm 1 end: [-0.167927857280008;10.7398650673619;1.270393476125]
Robot Arm 1 togoal: 10.1978900199287
Mobile Platform 2 Pose: [-2.38334933040627;-0.476480367178901;-9.64456648887995]
Robot Arm 2 end: [-0.0325440949310966;-9.64199253297056;1.12564404025217]
Robot Arm 1 togoal: 9.98171592968112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814617e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [0.819969855212821;0.0602244213745091;10.2679887547699]
Robot Arm 1 end: [0.0582821659434812;10.2645138610284;1.3119327688527]
Robot Arm 1 togoal: 9.94743334643732
Mobile Platform 2 Pose: [-2.38335472642266;-0.500483148957592;-9.92556887352686]
Robot Arm 2 end: [-0.237799888369222;-9.92753988047324;1.2698881791275]
Robot Arm 1 togoal: 9.76569806184011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823906e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [0.819952443538575;0.0828095620487168;10.7582418263909]
Robot Arm 1 end: [-0.142907872955345;10.7844799196128;1.26045669093914]
Robot Arm 1 togoal: 10.1765353475253
Mobile Platform 2 Pose: [-2.38335191760178;-0.521957736399613;-9.60719040226324]
Robot Arm 2 end: [-0.0201641444638724;-9.60827841654628;1.04886836968516]
Robot Arm 1 togoal: 9.9986738009964

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814801e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.813789e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [0.81997064382223;0.107325244639446;10.282416531563]
Robot Arm 1 end: [0.066501611446626;10.2769602317447;1.30883132087457]
Robot Arm 1 togoal: 9.93962337082056
Mobile Platform 2 Pose: [-2.38335487435198;-0.546112978527305;-9.91587842902993]
Robot Arm 2 end: [-0.214327480837149;-9.91640310238041;1.22885361171849]
Robot Arm 1 togoal: 9.79038940010951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823934e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [0.819950999542493;0.129995691670455;10.8022528128103]
Robot Arm 1 end: [-0.112328904759902;10.831835327435;1.25197247371302]
Robot Arm 1 togoal: 10.1500503299201
Mobile Platform 2 Pose: [-2.38335530950851;-0.567585860804379;-9.62286812676661]
Robot Arm 2 end: [-0.0280091563694388;-9.62955145576914;0.978882368918688]
Robot Arm 1 togoal: 9.99358419363662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814995e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814136e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [0.81997162818607;0.154442807170614;10.2943387138124]
Robot Arm 1 end: [0.079671274760692;10.286551713758;1.30382396851923]
Robot Arm 1 togoal: 9.92684236452436
Mobile Platform 2 Pose: [-2.38335475532443;-0.591637804483783;-9.91546607580472]
Robot Arm 2 end: [-0.192351397278835;-9.91508985216835;1.17517074642968]
Robot Arm 1 togoal: 9.81411120378653

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [0.819949383933275;0.177207732971175;10.8475834479512]
Robot Arm 1 end: [-0.075566025760763;10.8807832394234;1.24584137180234]
Robot Arm 1 togoal: 10.1177330526648
Mobile Platform 2 Pose: [-2.38439657916765;-0.642480996426504;-9.69836174569899]
Robot Arm 2 end: [-0.0771364640976857;-9.71078193058488;0.911475661438062]
Robot Arm 1 togoal: 9.94577235628864

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815693e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [0.819972905998779;0.201585892198949;10.3014448277967]
Robot Arm 1 end: [0.099324100433506;10.2908509569641;1.29800566340756]
Robot Arm 1 togoal: 9.90745699064982
Mobile Platform 2 Pose: [-2.38516104427899;-0.688816371247458;-9.93057342181427]
Robot Arm 2 end: [-0.238408311842512;-9.93021551034581;1.12188650720583]
Robot Arm 1 togoal: 9.76999660907986

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819300e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.824180e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [0.819947522180807;0.224456637421494;10.8932735315489]
Robot Arm 1 end: [-0.0310247213269071;10.9303792067146;1.24331380467164]
Robot Arm 1 togoal: 10.0779051420042
Mobile Platform 2 Pose: [-2.38620030579816;-0.760147567450108;-9.85860175344677]
Robot Arm 2 end: [-0.172379900127112;-9.87538479736753;0.814195673219358]
Robot Arm 1 togoal: 9.85379206866618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [0.819974634563921;0.248768700965113;10.3007335915788]
Robot Arm 1 end: [0.12790259517135;10.2866715935266;1.29280235013293]
Robot Arm 1 togoal: 9.87889477322056
Mobile Platform 2 Pose: [-2.38666623963767;-0.83882947293974;-9.87839156240167]
Robot Arm 2 end: [-0.317453515944447;-9.87971872485825;1.01988908892484]
Robot Arm 1 togoal: 9.69625113877755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818957e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.824589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [0.819945277639153;0.27176307248255;10.9385397578638]
Robot Arm 1 end: [0.0247392544451861;10.9798437483928;1.24622123759356]
Robot Arm 1 togoal: 10.0270346604238
Mobile Platform 2 Pose: [-2.38664844097847;-0.88786565188696;-10.0980652990847]
Robot Arm 2 end: [-0.296833774597509;-10.1152075838358;0.66587047575018]
Robot Arm 1 togoal: 9.7414554399437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818514e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [0.819977102443784;0.296016856933923;10.2875860740611]
Robot Arm 1 end: [0.169911518437962;10.2691369587231;1.29011849301969]
Robot Arm 1 togoal: 9.83648212157959
Mobile Platform 2 Pose: [-2.38664372699381;-0.911111595668148;-9.79785602010587]
Robot Arm 2 end: [-0.318955180248321;-9.80116814408179;0.811170409065849]
Robot Arm 1 togoal: 9.70906900568404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818519e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [0.819942386998024;0.31916939329947;10.9825167768475]
Robot Arm 1 end: [0.0994963243047389;11.0281782371257;1.2572881557353]
Robot Arm 1 togoal: 9.95724196033017
Mobile Platform 2 Pose: [-2.38664658911615;-0.932644653039063;-10.1466401588757]
Robot Arm 2 end: [-0.378533992747115;-10.1572078726756;0.513110368910026]
Robot Arm 1 togoal: 9.67538960209937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815678e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [0.819980882286802;0.34338029730831;10.2537206854397]
Robot Arm 1 end: [0.234620615531309;10.2297226603538;1.29223809939303]
Robot Arm 1 togoal: 9.77075938862765
Mobile Platform 2 Pose: [-2.38664281172715;-0.955704722356184;-9.80363027791785]
Robot Arm 2 end: [-0.366885030288084;-9.80406661991761;0.61701261844459]
Robot Arm 1 togoal: 9.67742151066748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817874e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.824114e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [0.819938329204183;0.366766293830787;11.022811855191]
Robot Arm 1 end: [0.211462759525745;11.0721415298934;1.27896102107589]
Robot Arm 1 togoal: 9.85005236714973
Mobile Platform 2 Pose: [-2.38664417105431;-0.977168032038403;-10.1451465338681]
Robot Arm 2 end: [-0.526036961401895;-10.1464638491699;0.341997773459641]
Robot Arm 1 togoal: 9.5481659791828

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820291e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [0.819987042924643;0.390961789312846;10.1840558557735]
Robot Arm 1 end: [0.342533471916732;10.1539846390636;1.29946779753052]
Robot Arm 1 togoal: 9.66123426518823
Mobile Platform 2 Pose: [-2.38664084395662;-0.99975168114602;-9.80925067316217]
Robot Arm 2 end: [-0.532253726771963;-9.80196117311606;0.347195419408632]
Robot Arm 1 togoal: 9.54228777938468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816789e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.825450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [0.819932216571242;0.41474339851711;11.0493054835811]
Robot Arm 1 end: [0.401419736794526;11.0976633478179;1.30142905767246]
Robot Arm 1 togoal: 9.66363385549427
Mobile Platform 2 Pose: [-2.38664014326576;-1.02074134176522;-10.1328059459671]
Robot Arm 2 end: [-0.8892724078857;-10.1199927441096;0.152454362371438]
Robot Arm 1 togoal: 9.21372197749769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815702e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.823323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [0.82001720872887;0.439024800393061;10.0678774457658]
Robot Arm 1 end: [0.532277020856317;10.0356112497723;1.29521611187438]
Robot Arm 1 togoal: 9.47048176889924
Mobile Platform 2 Pose: [-2.38663758999972;-1.04220945457819;-9.83695134829596]
Robot Arm 2 end: [-1.11429212209086;-9.81799405532559;0.11993111529111]
Robot Arm 1 togoal: 8.99732875219822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.815721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.830459e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [0.819947888248902;0.463194987962367;11.0364759513368]
Robot Arm 1 end: [0.666597221851478;11.067741149675;1.2706751960799]
Robot Arm 1 togoal: 9.39761358480532
Mobile Platform 2 Pose: [-2.38663616172117;-1.06139130985921;-10.1599010337088]
Robot Arm 2 end: [-1.62441399234377;-10.1390062695989;0.619744618074079]
Robot Arm 1 togoal: 8.42508308434315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816634e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.824999e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [0.820735755120221;0.514255480973721;9.95925118165609]
Robot Arm 1 end: [0.747802575094782;9.9370366846317;1.25929387927822]
Robot Arm 1 togoal: 9.25611212454388
Mobile Platform 2 Pose: [-2.38470982153749;-1.10647871516577;-9.93398524039048]
Robot Arm 2 end: [-1.68956504689273;-9.91125976312812;0.90426109109786]
Robot Arm 1 togoal: 8.33376090497087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.832651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [0.821197755792864;0.565016788135059;11.0377343710566]
Robot Arm 1 end: [0.801395455326539;11.0666534534586;1.25605906601888]
Robot Arm 1 togoal: 9.26403091842645
Mobile Platform 2 Pose: [-2.38072179219638;-1.16775745157486;-9.75402775539115]
Robot Arm 2 end: [-1.01545149974234;-9.73850462607462;1.28005495197577]
Robot Arm 1 togoal: 8.99158195759979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816458e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.818798e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [0.821988877490835;0.616249480399378;9.8303618941204]
Robot Arm 1 end: [0.893531158989296;9.81103046919888;1.23739375934887]
Robot Arm 1 togoal: 9.11284350439567
Mobile Platform 2 Pose: [-2.37866035426734;-1.22076677344449;-9.53622704413815]
Robot Arm 2 end: [-1.2870859298651;-9.52593889706513;1.31974138212147]
Robot Arm 1 togoal: 8.72812182282883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.832473e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [0.822438464809102;0.666867599836605;11.0084176237149]
Robot Arm 1 end: [0.934876440429995;11.0345774796873;1.24153343262186]
Robot Arm 1 togoal: 9.12824831356101
Mobile Platform 2 Pose: [-2.37452196037721;-1.28251825749024;-9.69697935453315]
Robot Arm 2 end: [-0.71050774162198;-9.69509197288731;0.839569830891445]
Robot Arm 1 togoal: 9.31944109905923

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [0.823221359770484;0.717573730352801;9.75323799841987]
Robot Arm 1 end: [0.975163292056808;9.73224630989631;1.24487665542699]
Robot Arm 1 togoal: 9.03302904595248
Mobile Platform 2 Pose: [-2.37250603486418;-1.31185047178306;-9.43753940733442]
Robot Arm 2 end: [-0.747998466133357;-9.4296280830573;0.902137077304153]
Robot Arm 1 togoal: 9.29020171058709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817206e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.827746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [0.823655029750423;0.768217053125913;11.0182913678911]
Robot Arm 1 end: [1.01815278359619;11.046194296872;1.24829778957819]
Robot Arm 1 togoal: 9.04668273021649
Mobile Platform 2 Pose: [-2.37251022997356;-1.33214043401;-9.79948431747097]
Robot Arm 2 end: [-0.827152732286797;-9.80213982022076;0.431766920399868]
Robot Arm 1 togoal: 9.23941038969647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816986e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_DCconstraint.m',55,1)">55  </a>    D2 = get_kmax_points(mobile_robot_next.car, mobile_robot_next.arm, obstacle_results);
D1

D1 =

   13.7943
         0
         0
         0

d_dmin

d_dmin =

    1.0382
    0.5288
    0.4275
    0.5174

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/get_dmin_points.m',34,1)">34  </a>end
D

D =

   13.8015
         0
         0
         0

obstacle_Q

obstacle_Q =

   -9.7157   -9.7157   -9.7157   -9.7157
   -9.7157   -9.7157   -9.7157   -9.7157
    1.2651    1.2651    1.2651    1.2651

mobilerobot_P

mobilerobot_P =

    0.2121   -0.1235   -0.1242    0.0006
   -0.2121   -0.0000   -0.0303    0.0297
         0    0.1573    0.1568    0.0410

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/get_dmin_points.m',35,1)">35  </a>end
d

d =

   13.7978   13.6946   13.6727   13.8123

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/get_dmin_points.m',35,1)">35  </a>end
d

d =

   13.7998
   13.6964
   13.6745
   13.8142

mobilerobot_P

mobilerobot_P =

    0.2121   -0.1235   -0.1242    0.0006
   -0.2121   -0.0000   -0.0303    0.0297
         0    0.1573    0.1568    0.0410

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/get_dmin_points.m',38,1)">38  </a>end
d

d =

   13.7989
   13.6956
   13.6737
   13.8134

mobilerobot_P

mobilerobot_P =

    0.2121   -0.1235   -0.1242    0.0006
   -0.2121   -0.0000   -0.0303    0.0297
         0    0.1573    0.1568    0.0410

obstacle_Q

obstacle_Q =

   -9.7157   -9.7157   -9.7157   -9.7157
   -9.7157   -9.7157   -9.7157   -9.7157
    1.2360    1.2360    1.2360    1.2360

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

if system_dependent('IsDebugMode')==1, dbcont; end
closest_arm_2_4_human

closest_arm_2_4_human = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

        name: 'human'
    human_id: 1
     seg_num: 2
    distance: 0.4118
      threat: 0
           s: 0.5000
           t: 0.5000
           P: [3×1 double]
           Q: [3×1 double]

min_distance.arm_2_4

ans =

    0.4118

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

mobile_robot_next
{函数或变量 'mobile_robot_next' 无法识别。
} 
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/get_dmin_points.m',38,1)">38  </a>end
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_DCconstraint.m',55,1)">55  </a>    D2 = get_kmax_points(mobile_robot_next.car, mobile_robot_next.arm, obstacle_results);
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',18,1)">18  </a>    X_d = pinv(mobile_robot.J)*Ve;
X_d = pinv(mobile_robot.J)*Ve

X_d =

 -804.8747
  817.1082
 -804.8764
  817.1099
   -0.0548
   97.8676
   -0.0548
   -0.0000
   -0.0548
  -97.8676
   -0.0548

Ve

Ve =

         0
         0
   -0.2193
   78.9058
   81.0992
         0

X_d = pinv(mobile_robot.J)*s_k

X_d =

   -6.8157
    6.9283
   -6.8157
    6.9283
   -0.0005
    0.9004
   -0.0005
   -0.0000
   -0.0005
   -0.9004
   -0.0005

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',18,1)">18  </a>    X_d = pinv(mobile_robot.J)*Ve;
X_d = pinv(mobile_robot.J)*s_k

X_d =

   -6.8157
    6.9283
   -6.8157
    6.9283
   -0.0005
    0.9004
   -0.0005
   -0.0000
   -0.0005
   -0.9004
   -0.0005

if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',32,1)">32  </a>    dx = fmincon(fun1, X_d, [], [], [], [], [], [], fun2, options);
mobile_robot.J*X_d

ans =

         0
    0.0000
   -0.0019
    0.7260
    0.6872
         0

mobile_robot.car.J*X_d(1:4)

ans =

   -0.0000
    0.0056
    0.6872

mobile_robot.arm.J_body*X_d(5:end)

ans =

         0
    0.0000
   -0.0019
    0.7203
         0
         0

if system_dependent('IsDebugMode')==1, dbquit all; end
r = 0.0475; 
l = 0.47/2;
w = 0.3/2;
F = r/4 * [-1/(l+w) 1/(l+w) 1/(l+w) -1/(l+w);1 1 1 1;-1 1 -1 1];
F

F =

   -0.0308    0.0308    0.0308   -0.0308
    0.0119    0.0119    0.0119    0.0119
   -0.0119    0.0119   -0.0119    0.0119

main_matlab
{无法识别的字段名称 "J_original"。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_platform_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_platform/mobile_platform_model.m', 47)" style="font-weight:bold">mobile_platform_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_platform/mobile_platform_model.m',47,0)">第 47 行</a>)
    car.J_original(:,3), ...  % ω_z分量

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m', 5)" style="font-weight:bold">mobile_robot_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',5,0)">第 5 行</a>)
    mobile_robot.car = mobile_platform_model();

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 11)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',11,0)">第 11 行</a>)
mobile_robot1 = mobile_robot_model();
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_platform/mobile_platform_model.m',47,0)
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('vertcat')" style="font-weight:bold">vertcat</a>
要串联的数组的维度不一致。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_model', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m', 17)" style="font-weight:bold">mobile_robot_model</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',17,0)">第 17 行</a>)
    J_base = Adjoint(inv(T_a_base) * inv(Tc2a)) * [zeros(1, 4); zeros(1, 4); mobile_robot.car.J; zeros(1, 4)];

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 11)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',11,0)">第 11 行</a>)
mobile_robot1 = mobile_robot_model();
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/mobile_robot_model.m',17,0)
main_matlab
if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',27,1)">27  </a>    dx = fmincon(fun1, X_d, [], [], [], [], [], [], fun2, options);
mobile_robot.J*X_d

ans =

         0
   -0.0000
   -0.0019
    0.7260
    0.6872
         0

mobile_robot.car.J*X_d(1:4)

ans =

   -0.0587
    0.0401
    0.6872

mobile_robot.arm.J_body*X_d(5:end)

ans =

         0
   -0.0000
    0.0568
    0.6859
         0
         0

mobile_robot.car.J*X_d(1:4)

ans =

   -0.0587
    0.0401
    0.6872

mobile_robot.J*X_d

ans =

         0
   -0.0000
   -0.0019
    0.7260
    0.6872
         0

mobile_robot.car.J*X_d(1:4)

ans =

   -0.0587
    0.0401
    0.6872

mobile_robot.arm.J_body*X_d(5:end)

ans =

         0
   -0.0000
    0.0568
    0.6859
         0
         0

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_DCconstraint.m',55,1)">55  </a>    D2 = get_kmax_points(mobile_robot_next.car, mobile_robot_next.arm, obstacle_results);
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000987e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.34228786648918;9.99051056373341;9.81851213808523]
Robot Arm 2 end: [9.76840306862366;9.82900641551575;1.48915542091185]
Robot Arm 1 togoal: 27.9996476654145

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.755921590689592;-9.97988497819246;-9.6577319006169]
Robot Arm 1 end: [-9.69473139479082;-9.65242476486688;1.46645126276626]
Robot Arm 1 togoal: 27.8227104972978
Mobile Platform 2 Pose: [-2.32894677747469;9.98141867227405;9.6316879644146]
Robot Arm 2 end: [9.55529407868931;9.62655577477263;1.39654297217669]
Robot Arm 1 togoal: 27.7060770885388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.741041397102319;-9.96973107815118;-9.48856967281555]
Robot Arm 1 end: [-9.55458065538716;-9.4798216906108;1.39996957464794]
Robot Arm 1 togoal: 27.6018065725759
Mobile Platform 2 Pose: [-2.31563722247766;9.97236223782736;9.44742119293667]
Robot Arm 2 end: [9.39107180463291;9.35633339302018;1.25924682401477]
Robot Arm 1 togoal: 27.3998142854895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978885e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.726085750816405;-9.95952595608916;-9.32064150140879]
Robot Arm 1 end: [-9.42892607377525;-9.30814325800548;1.31005200249116]
Robot Arm 1 togoal: 27.3921898268382
Mobile Platform 2 Pose: [-2.30196111619506;9.96304391747384;9.27602461175388]
Robot Arm 2 end: [9.29244546668457;9.10052688764908;1.11736167084375]
Robot Arm 1 togoal: 27.1512707505849

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969707e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.711071148155552;-9.94928082607102;-9.15383416545519]
Robot Arm 1 end: [-9.32182454994546;-9.13747218263378;1.19975272128423]
Robot Arm 1 togoal: 27.1970392744144
Mobile Platform 2 Pose: [-2.2879079459182;9.95346693483729;9.11722759667397]
Robot Arm 2 end: [9.24334343298604;8.84660596735799;0.970687187370825]
Robot Arm 1 togoal: 26.9407436267161

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.696014147992118;-9.93900693215315;-8.9880018026876]
Robot Arm 1 end: [-9.23670284449871;-8.96787640810329;1.07283142514188]
Robot Arm 1 togoal: 27.0191029982884
Mobile Platform 2 Pose: [-2.27317947345799;9.94346609269295;8.97338389362888]
Robot Arm 2 end: [9.27518262207936;8.54177030226247;0.780083798314948]
Robot Arm 1 togoal: 26.7559127647384

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.680930058676466;-9.92871465548676;-8.82297746775894]
Robot Arm 1 end: [-9.17623760051621;-8.79941220629422;0.933626381288688]
Robot Arm 1 togoal: 26.8605844236788
Mobile Platform 2 Pose: [-2.23295609426113;9.92073895904643;8.86037291364311]
Robot Arm 2 end: [9.35615422000722;8.30176370001074;0.628640142638255]
Robot Arm 1 togoal: 26.6535469841214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978033e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.665831815598383;-9.91841275221804;-8.6585868348378]
Robot Arm 1 end: [-9.14226012173675;-8.63212919847478;0.786906130707495]
Robot Arm 1 togoal: 26.7230472597311
Mobile Platform 2 Pose: [-2.19461480941764;9.89899649012528;8.77611742624426]
Robot Arm 2 end: [9.46953734794924;8.13392870814206;0.499961556985611]
Robot Arm 1 togoal: 26.6260168714226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.650729121699015;-9.90810777132205;-8.49466311568652]
Robot Arm 1 end: [-9.13568978254168;-8.4660764421974;0.6377047583955]
Robot Arm 1 togoal: 26.6073450969199
Mobile Platform 2 Pose: [-2.18084856084636;9.88961670756789;8.71677327350974]
Robot Arm 2 end: [9.6137919672996;8.0461674240304;0.370005591934606]
Robot Arm 1 togoal: 26.6775145424014

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.635627878258829;-9.89780367077227;-8.33106222595957]
Robot Arm 1 end: [-9.14979122173426;-8.30126073263887;0.493056253941508]
Robot Arm 1 togoal: 26.508627210497
Mobile Platform 2 Pose: [-2.16806395544214;9.88089969648488;8.67954451563289]
Robot Arm 2 end: [9.74532738621586;8.03693559025717;0.251905597102325]
Robot Arm 1 togoal: 26.7734868793625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000252e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.62042175742754;-9.88742786307749;-8.16786274884478]
Robot Arm 1 end: [-9.17946487028817;-8.13806180322454;0.357255935201147]
Robot Arm 1 togoal: 26.4233884832465
Mobile Platform 2 Pose: [-2.15648801143354;9.87299031235836;8.65915102472448]
Robot Arm 2 end: [9.78933205402901;8.03200231285109;0.222529236996842]
Robot Arm 1 togoal: 26.8040444126938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.605140705194117;-9.87700076826651;-8.00514089229303]
Robot Arm 1 end: [-9.18559970049075;-7.97434911767715;0.318955631894597]
Robot Arm 1 togoal: 26.3174727649618
Mobile Platform 2 Pose: [-2.14448102765071;9.86479546463817;8.6438675882312]
Robot Arm 2 end: [9.8606247090935;8.04977149692056;0.180273659515408]
Robot Arm 1 togoal: 26.8707315130528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992585e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.589843037460278;-9.86656233439354;-7.84333051322093]
Robot Arm 1 end: [-9.17526176801076;-7.81073011971916;0.318918031915389]
Robot Arm 1 togoal: 26.1984307226063
Mobile Platform 2 Pose: [-2.13271080110077;9.85676190527236;8.63744138319357]
Robot Arm 2 end: [9.924879521632;8.05845064520522;0.164898452466486]
Robot Arm 1 togoal: 26.9248486544323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971093e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.574476956223845;-9.85607729661586;-7.68269096600947]
Robot Arm 1 end: [-9.16488386391496;-7.64827828656266;0.318876634429779]
Robot Arm 1 togoal: 26.0806365064842
Mobile Platform 2 Pose: [-2.12137466169968;9.84903217510621;8.63928777500153]
Robot Arm 2 end: [10.012683636953;8.09891683365408;0.150834452628007]
Robot Arm 1 togoal: 27.0176914360109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981616e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.559043394587716;-9.84554629251811;-7.52323027028225]
Robot Arm 1 end: [-9.15446666727764;-7.48700179297147;0.318831395976652]
Robot Arm 1 togoal: 25.9640912486406
Mobile Platform 2 Pose: [-2.11187537499691;9.84254395094067;8.65638008185385]
Robot Arm 2 end: [10.054227761214;8.15196620877318;0.139052461148931]
Robot Arm 1 togoal: 27.0846027724836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988710e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.543543302255094;-9.83496997099686;-7.36495605264821]
Robot Arm 1 end: [-9.14401086728869;-7.3269084214144;0.318782272337449]
Robot Arm 1 togoal: 25.8487956198283
Mobile Platform 2 Pose: [-2.10356350868526;9.83685438574077;8.67946907021638]
Robot Arm 2 end: [10.0566170999609;8.20068833969474;0.127336800142556]
Robot Arm 1 togoal: 27.1196468016784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992030e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.527977644896094;-9.82434899182775;-7.20787553909628]
Robot Arm 1 end: [-9.13351716184729;-7.16800555442587;0.318729221279988]
Robot Arm 1 togoal: 25.7347498282938
Mobile Platform 2 Pose: [-2.09581103974425;9.83154507748697;8.70435400465325]
Robot Arm 2 end: [10.0539012704353;8.24616930346507;0.118744149169424]
Robot Arm 1 togoal: 27.1486274401905

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994398e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.512347403485289;-9.81368402521274;-7.05199554786409]
Robot Arm 1 end: [-9.12298625740851;-7.01030016733248;0.318672201689868]
Robot Arm 1 togoal: 25.6219536202526
Mobile Platform 2 Pose: [-2.08842311083819;9.82648370301315;8.72994231974719]
Robot Arm 2 end: [10.0495423105906;8.28721303707864;0.113166332591472]
Robot Arm 1 togoal: 27.1733012930478

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.496653573622041;-9.80297575131568;-6.89732248276789]
Robot Arm 1 end: [-9.11241886850294;-6.85379882146527;0.318611173604233]
Robot Arm 1 togoal: 25.5104062807587
Mobile Platform 2 Pose: [-2.08126492613815;9.82157849124743;8.7556673041443]
Robot Arm 2 end: [10.0445440966655;8.32354949898896;0.109891170612972]
Robot Arm 1 togoal: 27.1942538491763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997254e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.480897164832203;-9.79222485978517;-6.74386232701583]
Robot Arm 1 end: [-9.10181571724627;-6.69850765785465;0.318546098244226]
Robot Arm 1 togoal: 25.4001066352272
Mobile Platform 2 Pose: [-2.07423494511293;9.81676074176332;8.78122078297293]
Robot Arm 2 end: [10.0398566798259;8.35666229790039;0.107911681842996]
Robot Arm 1 togoal: 27.2132270240417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998879e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.465079199852494;-9.78143204926559;-6.59162063751894]
Robot Arm 1 end: [-9.09117753283955;-6.54443239142245;0.318476938046057]
Robot Arm 1 togoal: 25.2910530516085
Mobile Platform 2 Pose: [-2.06728980934075;9.81200082101095;8.80660614892686]
Robot Arm 2 end: [10.0350233603847;8.38711793011429;0.106713854568901]
Robot Arm 1 togoal: 27.2302859354846

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000162e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.449200713898937;-9.7705980268973;-6.44060253971254]
Robot Arm 1 end: [-9.08050505106126;-6.39157830568398;0.31840365669065]
Robot Arm 1 togoal: 25.1832434432081
Mobile Platform 2 Pose: [-2.06039116484397;9.80727254055664;8.83186268962596]
Robot Arm 2 end: [10.0298578741909;8.41567986402284;0.105943709220316]
Robot Arm 1 togoal: 27.2458227187314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.322441e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.433262753920773;-9.75972350780692;-6.29081272289949]
Robot Arm 1 end: [-9.06979901375214;-6.239950247972;0.318326219131776]
Robot Arm 1 togoal: 25.0766752721467
Mobile Platform 2 Pose: [-2.05350962866431;9.80255583825253;8.85703040736768]
Robot Arm 2 end: [10.0243565036556;8.44306460006863;0.105389717869721]
Robot Arm 1 togoal: 27.260327175195

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.873128e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.417266377841304;-9.74880921458877;-6.14225543612552]
Robot Arm 1 end: [-9.05906016829367;-6.08955262519274;0.318244591622641]
Robot Arm 1 togoal: 24.9713455534512
Mobile Platform 2 Pose: [-2.04662542013649;9.797837257481;8.88212986528832]
Robot Arm 2 end: [10.0186912461544;8.46989427232354;0.104919194929986]
Robot Arm 1 togoal: 27.2743520268797

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.470204e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.401212653787188;-9.73785587677836;-5.99493448459504]
Robot Arm 1 end: [-9.04828926708141;-5.94038940012288;0.318158741740877]
Robot Arm 1 togoal: 24.8672508597659
Mobile Platform 2 Pose: [-2.03972991241122;9.79311093280681;8.90718166901416]
Robot Arm 2 end: [10.0129692153308;8.49667203624865;0.104446874643369]
Robot Arm 1 togoal: 27.2883205050674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.671905e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.385102659307702;-9.72686423031915;-5.8488532266347]
Robot Arm 1 end: [-9.03748706699411;-5.79246408825485;0.318068638411877]
Robot Arm 1 togoal: 24.7643873266729
Mobile Platform 2 Pose: [-2.03282313514187;9.78837688342526;8.93218975271362]
Robot Arm 2 end: [10.0072378332708;8.52378053284772;0.10391038329826]
Robot Arm 1 togoal: 27.3025301744716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999330e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.368937480585549;-9.71583501702348;-5.70401457121015]
Robot Arm 1 end: [-9.02665432885965;-5.64577975519625;0.31797425193045]
Robot Arm 1 togoal: 24.6627506586054
Mobile Platform 2 Pose: [-2.02591167865104;9.78363958285546;8.95714012461813]
Robot Arm 2 end: [10.001475025513;8.55145167356833;0.103264331110656]
Robot Arm 1 togoal: 27.3171253396071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997113e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.352718211640781;-9.7047689840289;-5.56042097600019]
Robot Arm 1 end: [-9.01579181691897;-5.50033901462767;0.317875553980748]
Robot Arm 1 togoal: 24.5623361353394
Mobile Platform 2 Pose: [-2.01900614233508;9.77890624018266;8.98200882479272]
Robot Arm 2 end: [9.99557834622445;8.57971726996201;0.102490474740567]
Robot Arm 1 togoal: 27.3320547435603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996160e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.336445953529443;-9.69366688325088;-5.41807444603074]
Robot Arm 1 end: [-9.00490029828894;-5.35614402682166;0.317772517654445]
Robot Arm 1 togoal: 24.4631386190461
Mobile Platform 2 Pose: [-2.01211727170056;9.77418420287139;9.00676925839501]
Robot Arm 2 end: [9.98947270661028;8.60845683249844;0.10159768377481]
Robot Arm 1 togoal: 27.347182445557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995135e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.32012181353854;-9.68252947083301;-5.27697653286952]
Robot Arm 1 end: [-8.99398054242525;-5.213196497724;0.317665117467128]
Robot Arm 1 togoal: 24.3651525618846
Mobile Platform 2 Pose: [-2.00525233775768;9.76947879387965;9.03141318492973]
Robot Arm 2 end: [9.98368256234699;8.63806743789669;0.100504701886415]
Robot Arm 1 togoal: 27.363167460369

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994386e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.303746904378936;-9.67135750659591;-5.13712833438094]
Robot Arm 1 end: [-8.98303332058644;-5.07149767859683;0.317553329372896]
Robot Arm 1 togoal: 24.2683720141152
Mobile Platform 2 Pose: [-1.99843552592094;9.76480653500643;9.05611647767257]
Robot Arm 2 end: [9.97813916765109;8.66829254099013;0.0993102861535985]
Robot Arm 1 togoal: 27.3797807266691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993943e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.287322343377793;-9.66015175348576;-4.99853049503885]
Robot Arm 1 end: [-8.97205940530007;-4.93104836622174;0.317437130777141]
Robot Arm 1 togoal: 24.1727906327106
Mobile Platform 2 Pose: [-1.99168229624244;9.76017790633025;9.08104240028148]
Robot Arm 2 end: [9.97273128710458;8.69886221681041;0.0981136205367688]
Robot Arm 1 togoal: 27.3967525250532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.270849251672138;-9.64891297702367;-4.86118320679359]
Robot Arm 1 end: [-8.961059569832;-4.79184890365925;0.3173165005475]
Robot Arm 1 togoal: 24.0784016904415
Mobile Platform 2 Pose: [-1.98499804778892;9.75559651249166;9.10629859094343]
Robot Arm 2 end: [9.96732940910904;8.72952029141602;0.0969943587068495]
Robot Arm 1 togoal: 27.4138097707136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.254328753405152;-9.63764194475692;-4.72508621048801]
Robot Arm 1 end: [-8.95003458765994;-4.65389918155965;0.317191419022983]
Robot Arm 1 togoal: 23.9851980854121
Mobile Platform 2 Pose: [-1.97837947015595;9.75105996504966;9.13192092365647]
Robot Arm 2 end: [9.96181511101404;8.76001242722908;0.0960098289065496]
Robot Arm 1 togoal: 27.4306894879405

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.237761974926717;-9.62633942571313;-4.59023879781584]
Robot Arm 1 end: [-8.93898523195212;-4.51719864001875;0.317061868021266]
Robot Arm 1 togoal: 23.8931723510189
Mobile Platform 2 Pose: [-1.97181599706918;9.7465609295837;9.15788078314609]
Robot Arm 2 end: [9.95610421523549;8.7900771448367;0.0951974239769575]
Robot Arm 1 togoal: 27.4471493518735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.221150043999774;-9.61500618985849;-4.45663981381407]
Robot Arm 1 end: [-8.92791227505214;-4.38174627097048;0.316927830844163]
Robot Arm 1 togoal: 23.8023166663075
Mobile Platform 2 Pose: [-1.96535170602871;9.74211839654141;9.18410414345534]
Robot Arm 2 end: [9.93788875117095;8.79880781915157;0.099423972220732]
Robot Arm 1 togoal: 27.4396694892307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988305e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.204494089013975;-9.60364300756088;-4.3242876598801]
Robot Arm 1 end: [-8.91681648797091;-4.24754062110698;0.316789292281287]
Robot Arm 1 togoal: 23.7126228666979
Mobile Platform 2 Pose: [-1.95832608246459;9.73730289046033;9.20441220529255]
Robot Arm 2 end: [9.93241137689111;8.82219090890193;0.0987564926876174]
Robot Arm 1 togoal: 27.4517516836879

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.250442e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.187795238208097;-9.59225064905909;-4.19318029730241]
Robot Arm 1 end: [-8.90569863988658;-4.11457979531553;0.316646238611902]
Robot Arm 1 togoal: 23.6240824550523
Mobile Platform 2 Pose: [-1.95132421134209;9.73250403635062;9.22512073949535]
Robot Arm 2 end: [9.9275494987354;8.84545269540091;0.0981424147844633]
Robot Arm 1 togoal: 27.464210165722
Step 41:
Mobile Platform 1 Pose: [0.17105461890263;-9.58082988393898;-4.06331525129281]
Robot Arm 1 end: [-8.89455949765334;-3.98286146062012;0.31649865760501]
Robot Arm 1 togoal: 23.5366866130534
Mobile Platform 2 Pose: [-1.94434980083763;9.72772292157406;9.24645544905684]
Robot Arm 2 end: [9.92055131256619;8.8652704245353;0.0979465872838403]
Robot Arm 1 togoal: 27.4727478385922
Step 42:
Mobile Platform 1 Pose: [0.15427335674391;-9.56938148061755;-3.93468961550677]
Robot Arm 1 end: [-8.88339982532;-3.85238285061453;0.316346538517676]
Robot Arm 1 togoal: 23.4504262128678
Mobile Platform 2 Pose: [-1.93729979849326;9.72289019818464;9.26786482962301]
Robot Arm 2 end: [9.91348335026166;8.88517221037554;0.0977535593406842]
Robot Arm 1 togoal: 27.4813059913014
Step 43:
Mobile Platform 1 Pose: [0.137452574961122;-9.55790620583581;-3.80730005703718]
Robot Arm 1 end: [-8.87222038365899;-3.72314077037239;0.316189872091636]
Robot Arm 1 togoal: 23.3652918290605
Mobile Platform 2 Pose: [-1.9301738656999;9.71800563603633;9.28934170937739]
Robot Arm 2 end: [9.90634539774471;8.90515113644681;0.0975633303534186]
Robot Arm 1 togoal: 27.4898799295068
Step 44:
Mobile Platform 1 Pose: [0.120593393637415;-9.54640482416134;-3.68114282186597]
Robot Arm 1 end: [-8.86102192970673;-3.59513160181865;0.316028650548207]
Robot Arm 1 togoal: 23.2812737507346
Mobile Platform 2 Pose: [-1.92297167226147;9.71306901074335;9.31087878700427]
Robot Arm 2 end: [9.89913725086316;8.92520015833973;0.0973758991562368]
Robot Arm 1 togoal: 27.4984648651959

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.536188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.106182898870431;-9.53612108201537;-3.55621373960584]
Robot Arm 1 end: [-8.87971362079574;-3.46239943410699;0.272871268845511]
Robot Arm 1 togoal: 23.2214902677722
Mobile Platform 2 Pose: [-1.91569289667337;9.70808010387029;9.33246863262874]
Robot Arm 2 end: [9.89185871564925;8.94531210457216;0.0971912640186982]
Robot Arm 1 togoal: 27.5070559169762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.800250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.0914668756022717;-9.52567696691382;-3.43222443892893]
Robot Arm 1 end: [-8.91807311457893;-3.32856866085663;0.210598291648846]
Robot Arm 1 togoal: 23.1788995759715
Mobile Platform 2 Pose: [-1.90833722640059;9.7030387031218;9.35410368887595]
Robot Arm 2 end: [9.88450960857748;8.96547967756743;0.0970094226458516]
Robot Arm 1 togoal: 27.5156481104521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.058886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.077497163805676;-9.51559769068871;-3.30960415008107]
Robot Arm 1 end: [-8.93529463546915;-3.19023681534007;0.183786582200649]
Robot Arm 1 togoal: 23.1152735199375
Mobile Platform 2 Pose: [-1.90090435815493;9.69794460253183;9.37577627205165]
Robot Arm 2 end: [9.87708975682041;8.98569545475084;0.0968303721789012]
Robot Arm 1 togoal: 27.5242363786909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.0630108176989757;-9.50524638817556;-3.19063089387247]
Robot Arm 1 end: [-8.9725562335862;-3.06311077394755;0.139354953322782]
Robot Arm 1 togoal: 23.0762149444839
Mobile Platform 2 Pose: [-1.89339399817123;9.69279760265205;9.39747857344557]
Robot Arm 2 end: [9.86959899850212;9.00595188976612;0.09665410919643]
Robot Arm 1 togoal: 27.5328155627794

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.940472e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.0842050299587905;-9.51272375666294;-3.07430465370191]
Robot Arm 1 end: [-8.92642241220904;-2.84615896241996;0.227976149372716]
Robot Arm 1 togoal: 22.910809154009
Mobile Platform 2 Pose: [-1.88580586248215;9.68759751073932;9.41920266075913]
Robot Arm 2 end: [9.86203718294902;9.02624131381355;0.0964806297161938]
Robot Arm 1 togoal: 27.5413804124744

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996204e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.070093048936273;-9.50255626558065;-2.98115021091326]
Robot Arm 1 end: [-8.97173784517571;-2.76231821860413;0.165558284488533]
Robot Arm 1 togoal: 22.9050393019339
Mobile Platform 2 Pose: [-1.87813967719122;9.68234414094201;9.44094047965909]
Robot Arm 2 end: [9.85440417093767;9.04655593711083;0.0963099291974949]
Robot Arm 1 togoal: 27.5499255869464

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996682e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.056108759242382;-9.49245048938103;-2.88915509667673]
Robot Arm 1 end: [-9.01379324311667;-2.67761096814211;0.118681846437698]
Robot Arm 1 togoal: 22.895690632854
Mobile Platform 2 Pose: [-1.87039517874385;9.67703731448484;9.46268385545811]
Robot Arm 2 end: [9.84669983493927;9.06688785047836;0.096142002544153]
Robot Arm 1 togoal: 27.5584456556209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.915421e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.0759968009006338;-9.49927869837132;-2.79938382765572]
Robot Arm 1 end: [-8.97187015530853;-2.4908782924311;0.208964021744104]
Robot Arm 1 togoal: 22.7524797794537
Mobile Platform 2 Pose: [-1.86257211419598;9.67167685985214;9.48442449492341]
Robot Arm 2 end: [9.83892405936049;9.08722902704965;0.0959768441080793]
Robot Arm 1 togoal: 27.5669350991173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994059e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.0634716895139408;-9.48998261292912;-2.72802267654748]
Robot Arm 1 end: [-8.9925739304513;-2.42167418551237;0.179183519475774]
Robot Arm 1 togoal: 22.7335946462958
Mobile Platform 2 Pose: [-1.85465768668396;9.6662563355712;9.50615401698921]
Robot Arm 2 end: [9.83551549442477;9.12468051976411;0.0935283821378881]
Robot Arm 1 togoal: 27.590555542294

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.0505924317553015;-9.48051921321315;-2.65901766625651]
Robot Arm 1 end: [-9.03205102340365;-2.37105652497348;0.129351763770591]
Robot Arm 1 togoal: 22.7420027815258
Mobile Platform 2 Pose: [-1.84702494176917;9.66102989956598;9.52929367881769]
Robot Arm 2 end: [9.83379912948209;9.16515007307322;0.0912048429792677]
Robot Arm 1 togoal: 27.6175103769891

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.922804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.0633297184152031;-9.48386803628991;-2.59156811738522]
Robot Arm 1 end: [-8.97846134427486;-2.22061757248504;0.22805405470366]
Robot Arm 1 togoal: 22.6096704546202
Mobile Platform 2 Pose: [-1.83975068731894;9.65604475863247;9.55461182849205]
Robot Arm 2 end: [9.82661325588708;9.18901917690221;0.0909997890855741]
Robot Arm 1 togoal: 27.6289332181547

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.052452854399396;-9.47550069549096;-2.53388438248511]
Robot Arm 1 end: [-8.98609210891641;-2.15959975039086;0.213317883049369]
Robot Arm 1 togoal: 22.584011870723
Mobile Platform 2 Pose: [-1.83240006254236;9.65100749429094;9.57995568592821]
Robot Arm 2 end: [9.81935787891051;9.21292451103533;0.090797820298893]
Robot Arm 1 togoal: 27.6403490078164

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997320e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.0414949188450164;-9.46711352761833;-2.47825515810567]
Robot Arm 1 end: [-9.00180987966231;-2.11013949819908;0.186074046136352]
Robot Arm 1 togoal: 22.5722458933747
Mobile Platform 2 Pose: [-1.82497279347033;9.64591792057417;9.60531768859117]
Robot Arm 2 end: [9.81203284945947;9.23685875892281;0.0905989364462188]
Robot Arm 1 togoal: 27.6517527064999

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877810e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.0254067129354338;-9.45617609635817;-2.42413716981631]
Robot Arm 1 end: [-8.98564166765116;-2.14458027073352;0.137709816828814]
Robot Arm 1 togoal: 22.5800556070086
Mobile Platform 2 Pose: [-1.81746861581322;9.64077585813612;9.63069016306219]
Robot Arm 2 end: [9.80463802921984;9.26081449398388;0.0904031369017541]
Robot Arm 1 togoal: 27.663139189629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.706319e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.00784001906071104;-9.44438332885453;-2.35477407418696]
Robot Arm 1 end: [-8.8774036362111;-2.15050234180298;0.208792441119733]
Robot Arm 1 togoal: 22.4880626973552
Mobile Platform 2 Pose: [-1.80988727520544;9.63558113441871;9.65606532669553]
Robot Arm 2 end: [9.79717329087575;9.28478418117705;0.0902104205900757]
Robot Arm 1 togoal: 27.6745032483708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811938e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-0.00953214288131167;-9.43254324987551;-2.25659467018095]
Robot Arm 1 end: [-8.91663348101162;-2.0955336087201;0.143598619966018]
Robot Arm 1 togoal: 22.4952926208202
Mobile Platform 2 Pose: [-1.80222852744738;9.63033358381689;9.68143528938713]
Robot Arm 2 end: [9.78963851832531;9.3087601786801;0.0900207859897479]
Robot Arm 1 togoal: 27.6858395905686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005043e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-0.0273049860144744;-9.42049205485624;-2.15026583209803]
Robot Arm 1 end: [-8.83999479448346;-2.06873822846121;0.206731795602358]
Robot Arm 1 togoal: 22.4126559603358
Mobile Platform 2 Pose: [-1.79449213874459;9.62503304784167;9.70679205545564]
Robot Arm 2 end: [9.78203360689273;9.33273473967999;0.0898342311373935]
Robot Arm 1 togoal: 27.6971428417631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.042370e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-0.0445328057700956;-9.40873219862474;-2.03021271999775]
Robot Arm 1 end: [-8.78335241122935;-1.90757801481395;0.285298116203185]
Robot Arm 1 togoal: 22.2740140327004
Mobile Platform 2 Pose: [-1.78667788594375;9.61967937528106;9.73212752563608]
Robot Arm 2 end: [9.77435846353611;9.35670001427322;0.0896507536322237]
Robot Arm 1 togoal: 27.7084075463038

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.529141e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-0.0618709066764987;-9.39689608074059;-1.92510832444028]
Robot Arm 1 end: [-8.73118018455217;-1.76660435831727;0.356069822733546]
Robot Arm 1 togoal: 22.1510079127167
Mobile Platform 2 Pose: [-1.77878555676524;9.61427242235859;9.7574334991862]
Robot Arm 2 end: [9.76661300705095;9.38064805147661;0.0894703506410334]
Robot Arm 1 togoal: 27.7196281685508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-0.0792524150162157;-9.38504141980732;-1.82927495965844]
Robot Arm 1 end: [-8.77018474624902;-1.6768166343688;0.266593317969951]
Robot Arm 1 togoal: 22.1413960319785
Mobile Platform 2 Pose: [-1.77081495003192;9.60881205288935;9.78270167610568]
Robot Arm 2 end: [9.75879716826881;9.4045708013486;0.0892930189036606]
Robot Arm 1 togoal: 27.7307990941671

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-0.0962108203615317;-9.37338444706715;-1.729988735415]
Robot Arm 1 end: [-8.75727705090853;-1.45672342722113;0.342185981315184]
Robot Arm 1 togoal: 22.0109417459368
Mobile Platform 2 Pose: [-1.76276587589387;9.60329813843314;9.80792365946754]
Robot Arm 2 end: [9.75091089025106;9.42846011722093;0.0891187547389172]
Robot Arm 1 togoal: 27.7419146315032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-0.113166475238675;-9.36182298556153;-1.65810628439338]
Robot Arm 1 end: [-8.78555485968797;-1.38099417061345;0.277058197598885]
Robot Arm 1 togoal: 21.9993520564958
Mobile Platform 2 Pose: [-1.75463815604881;9.59773055844472;9.83309095786178]
Robot Arm 2 end: [9.74295412847718;9.45230775804009;0.0889475540509905]
Robot Arm 1 togoal: 27.7529690130733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-0.129338709551051;-9.35065756728173;-1.58795020551201]
Robot Arm 1 end: [-8.80834404998262;-1.19104531037575;0.344347338916221]
Robot Arm 1 togoal: 21.9175229559874
Mobile Platform 2 Pose: [-1.74643162395781;9.59210920042081;9.85819498795032]
Robot Arm 2 end: [9.73492685102756;9.47610539081829;0.0887794123363134]
Robot Arm 1 togoal: 27.7639563971239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000169e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-0.144819441685045;-9.34010281622435;-1.54234203733106]
Robot Arm 1 end: [-8.83891694794318;-1.12086377859779;0.303771819402709]
Robot Arm 1 togoal: 21.9102726454344
Mobile Platform 2 Pose: [-1.73814612505607;9.58643396004379;9.8832270771325]
Robot Arm 2 end: [9.72682903876027;9.49984459319296;0.0886143246909096]
Robot Arm 1 togoal: 27.7748708692945

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000399e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-0.159856355179038;-9.32984904155896;-1.5028743001413]
Robot Arm 1 end: [-8.89245618181077;-1.06456539543872;0.24383998703534]
Robot Arm 1 togoal: 21.9312709542857
Mobile Platform 2 Pose: [-1.72978151695841;9.58070474132163;9.90817846632029]
Robot Arm 2 end: [9.71866068548167;9.52351685609403;0.0884522858182057]
Robot Arm 1 togoal: 27.7857064443709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002973e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-0.17420912647838;-9.32005707157569;-1.47062364888966]
Robot Arm 1 end: [-8.92290252639686;-0.949203407498716;0.284109904827423]
Robot Arm 1 togoal: 21.8972873294807
Mobile Platform 2 Pose: [-1.72133766965924;9.57492145672421;9.93304031282178]
Robot Arm 2 end: [9.71042179811053;9.54711358651768;0.0882932900373125]
Robot Arm 1 togoal: 27.7964570681306

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003958e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-0.184801924570687;-9.31239309368375;-1.4479896233217]
Robot Arm 1 end: [-8.90322010764957;-0.923743122524617;0.291669015666732]
Robot Arm 1 togoal: 21.8671248433394
Mobile Platform 2 Pose: [-1.71281446572661;9.56908402731544;9.95780369335055]
Robot Arm 2 end: [9.70211239675461;9.57062611036489;0.0881373312892463]
Robot Arm 1 togoal: 27.8071166191936

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003965e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999171e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-0.202188927875833;-9.30134355683786;-1.42496451125422]
Robot Arm 1 end: [-8.90373284218247;-0.898503199183979;0.271288752941126]
Robot Arm 1 togoal: 21.8561264992738
Mobile Platform 2 Pose: [-1.70421468616038;9.56319382571591;9.98245960704682]
Robot Arm 2 end: [9.68329368817625;9.62909107332641;0.0668465158417152]
Robot Arm 1 togoal: 27.8360886105687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003904e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000279e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-0.225017780574365;-9.28765317424959;-1.4036188317364]
Robot Arm 1 end: [-8.92109962598408;-0.871860061283082;0.240805077790668]
Robot Arm 1 togoal: 21.8596489287314
Mobile Platform 2 Pose: [-1.69180047446806;9.55543587825235;10.0075241519524]
Robot Arm 2 end: [9.64755165939127;9.62130255463377;0.0829708915243064]
Robot Arm 1 togoal: 27.8044911456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003736e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000195e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-0.251552222971217;-9.27225395377244;-1.3852643338413]
Robot Arm 1 end: [-8.96046509603934;-0.840450579217286;0.210288307661188]
Robot Arm 1 togoal: 21.8799581937218
Mobile Platform 2 Pose: [-1.67754589163719;9.54663459146155;10.0298636805474]
Robot Arm 2 end: [9.60154371635633;9.63442677575509;0.0812777011650551]
Robot Arm 1 togoal: 27.7813612188906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004978e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000249e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-0.266971121818378;-9.26263190750078;-1.37028156820601]
Robot Arm 1 end: [-8.97467817148438;-0.808865692977273;0.209471861181646]
Robot Arm 1 togoal: 21.8767021129589
Mobile Platform 2 Pose: [-1.66253443428224;9.53734973459906;10.0484647493217]
Robot Arm 2 end: [9.55608503085489;9.64186219697043;0.0810573473537354]
Robot Arm 1 togoal: 27.7545788500152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002981e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000040e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-0.281517363211181;-9.25355328795014;-1.35670066922486]
Robot Arm 1 end: [-9.01011953186558;-0.817634531550029;0.168234108547893]
Robot Arm 1 togoal: 21.9142838585037
Mobile Platform 2 Pose: [-1.64727913562952;9.52783040721035;10.0626218137316]
Robot Arm 2 end: [9.51998272290515;9.70692030206916;0.046710999524278]
Robot Arm 1 togoal: 27.7770761843565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002463e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000160e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-0.295465548792693;-9.24492008087499;-1.34492550771286]
Robot Arm 1 end: [-9.05485167250677;-0.829684105439045;0.135619599669319]
Robot Arm 1 togoal: 21.9610725816945
Mobile Platform 2 Pose: [-1.63142236876896;9.51799976741861;10.0742215304693]
Robot Arm 2 end: [9.48471388398911;9.74424602364839;0.0293831023925705]
Robot Arm 1 togoal: 27.7797452679556

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001786e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000373e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-0.308682780469324;-9.23681814172651;-1.33437961742347]
Robot Arm 1 end: [-9.11091225747791;-0.844604490077372;0.110686820211494]
Robot Arm 1 togoal: 22.0186602027533
Mobile Platform 2 Pose: [-1.61494594156947;9.50780619460758;10.0821536284241]
Robot Arm 2 end: [9.44784245495688;9.74772530231675;0.0274310294290744]
Robot Arm 1 togoal: 27.7564763440613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001074e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988474e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-0.320682541652194;-9.22951137702855;-1.32415343438849]
Robot Arm 1 end: [-9.17112937844031;-0.862520348602008;0.0937268000399167]
Robot Arm 1 togoal: 22.0808437057105
Mobile Platform 2 Pose: [-1.60861055454024;9.50256749313125;10.0851988424903]
Robot Arm 2 end: [9.35971930429084;9.67101501987897;0.0866352578175157]
Robot Arm 1 togoal: 27.6370216497157

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000389e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-0.331416851543866;-9.22302162426439;-1.31293081802473]
Robot Arm 1 end: [-9.23230635968858;-0.882894534868477;0.0834258016396117]
Robot Arm 1 togoal: 22.1446521974444
Mobile Platform 2 Pose: [-1.59709336426132;9.49456239198066;10.0601731688158]
Robot Arm 2 end: [9.29289880127015;9.68185276775647;0.0853374209372936]
Robot Arm 1 togoal: 27.5980506479826

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996487e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-0.341236984916514;-9.21716104136443;-1.29924667043412]
Robot Arm 1 end: [-9.29766496465365;-0.905920458271745;0.0785827281604559]
Robot Arm 1 togoal: 22.2130489063584
Mobile Platform 2 Pose: [-1.58330609240363;9.48545711021237;10.0105307165963]
Robot Arm 2 end: [9.24221002909655;9.66838112768088;0.0839896315917304]
Robot Arm 1 togoal: 27.553091691977

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991958e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-0.349676919128057;-9.21215609214792;-1.28124513261072]
Robot Arm 1 end: [-9.35507174499738;-0.928191650301387;0.0780859668484087]
Robot Arm 1 togoal: 22.2738900039212
Mobile Platform 2 Pose: [-1.56847399727072;9.47588387297171;9.94337169296672]
Robot Arm 2 end: [9.22092466037206;9.62636481539063;0.0791315021424353]
Robot Arm 1 togoal: 27.5084918785385

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-0.357771093357818;-9.20745560944348;-1.25767951575946]
Robot Arm 1 end: [-9.41465869505416;-0.95273015794471;0.0815260894374988]
Robot Arm 1 togoal: 22.3374876675374
Mobile Platform 2 Pose: [-1.53920472685277;9.45915260124186;9.86928652024696]
Robot Arm 2 end: [9.22663542887776;9.59429695226064;0.0611488825246774]
Robot Arm 1 togoal: 27.4905647132528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-0.364712933320354;-9.2034505650492;-1.22645248307757]
Robot Arm 1 end: [-9.46322678983216;-0.969600845675458;0.0873443334414502]
Robot Arm 1 togoal: 22.3876016660146
Mobile Platform 2 Pose: [-1.52849540751346;9.45181949598678;9.7906206070484]
Robot Arm 2 end: [9.23418579439306;9.54491161924691;0.057818651806017]
Robot Arm 1 togoal: 27.4608516443884

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-0.370816870623788;-9.19994727343573;-1.18704877628368]
Robot Arm 1 end: [-9.50237543638288;-0.97700967944947;0.0944989994705644]
Robot Arm 1 togoal: 22.4248142527172
Mobile Platform 2 Pose: [-1.51820571328142;9.44479649189398;9.71734114514506]
Robot Arm 2 end: [9.2591217287193;9.48490703699797;0.0573596611102685]
Robot Arm 1 togoal: 27.4356996456024

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986108e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-0.377842646155152;-9.19603791667433;-1.13939703000575]
Robot Arm 1 end: [-9.54602651940089;-0.994413358657419;0.106325411232637]
Robot Arm 1 togoal: 22.4705492142549
Mobile Platform 2 Pose: [-1.50835176805331;9.43807915355222;9.66474690361835]
Robot Arm 2 end: [9.27663793439741;9.44688988898449;0.0611755162129123]
Robot Arm 1 togoal: 27.4208202636537

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999146e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-0.384101177672789;-9.19255720711836;-1.0804839123128]
Robot Arm 1 end: [-9.57770807834566;-0.990649487171125;0.118239145576093]
Robot Arm 1 togoal: 22.4955277280948
Mobile Platform 2 Pose: [-1.49904733726197;9.43172931722691;9.63167335231676]
Robot Arm 2 end: [9.28931492825977;9.43098891986286;0.0628203976285854]
Robot Arm 1 togoal: 27.4183750167327

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996999e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-0.38595640768186;-9.19129990999604;-1.01173941841345]
Robot Arm 1 end: [-9.57686885484481;-0.920980385562084;0.118106105465234]
Robot Arm 1 togoal: 22.4608485596815
Mobile Platform 2 Pose: [-1.49020120133695;9.42567395972227;9.60872508474384]
Robot Arm 2 end: [9.29265023917232;9.41493514241989;0.0621447403262569]
Robot Arm 1 togoal: 27.4093837673174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949030e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-0.387828946139769;-9.19003081485999;-0.942378000887291]
Robot Arm 1 end: [-9.57601850902568;-0.850693463646294;0.117975373146826]
Robot Arm 1 togoal: 22.4260234417805
Mobile Platform 2 Pose: [-1.47616088022972;9.41704327765471;9.58854163789323]
Robot Arm 2 end: [9.22208840114635;9.32552467768682;0.0754104105942677]
Robot Arm 1 togoal: 27.2956830988046

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.944056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-0.389718771908377;-9.18874993629119;-0.872390739201089]
Robot Arm 1 end: [-9.57515704681714;-0.779779794766112;0.117846956905244]
Robot Arm 1 togoal: 22.3910532198365
Mobile Platform 2 Pose: [-1.46117797685017;9.40769079755948;9.56470754442483]
Robot Arm 2 end: [9.1883831234418;9.27793739904596;0.0750798314122395]
Robot Arm 1 togoal: 27.2382745853597

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.936527e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-0.391625862538306;-9.18745728976655;-0.801768578319619]
Robot Arm 1 end: [-9.57428447481925;-0.708230317607264;0.117720864985385]
Robot Arm 1 togoal: 22.3559388433329
Mobile Platform 2 Pose: [-1.44560535818576;9.39788192803286;9.53661036446723]
Robot Arm 2 end: [9.15621363018083;9.22860519140313;0.0774975698126147]
Robot Arm 1 togoal: 27.1805716922573

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-0.393550194207945;-9.18615289170047;-0.730502326567792]
Robot Arm 1 end: [-9.57340080033745;-0.63603583405822;0.117597105591445]
Robot Arm 1 togoal: 22.3206813703806
Mobile Platform 2 Pose: [-1.42956026599122;9.38769490349471;9.50409372434463]
Robot Arm 2 end: [9.12571780161278;9.17740056394252;0.0818847182647309]
Robot Arm 1 togoal: 27.1226257322361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.918654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-0.395491741660064;-9.18483675948814;-0.658582653477802]
Robot Arm 1 end: [-9.57250603141831;-0.563187007054596;0.117475686885638]
Robot Arm 1 togoal: 22.2852819724906
Mobile Platform 2 Pose: [-1.41314998102155;9.37720206434759;9.46716108345455]
Robot Arm 2 end: [9.09692779397809;9.12423369756416;0.0875809348229514]
Robot Arm 1 togoal: 27.0644352592821

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.936281e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-0.397450478135946;-9.18350891155041;-0.586000087622851]
Robot Arm 1 end: [-9.57160017688651;-0.489674358410182;0.117356616986844]
Robot Arm 1 togoal: 22.2497419395387
Mobile Platform 2 Pose: [-1.39646974135402;9.36646820841273;9.42593325860743]
Robot Arm 2 end: [9.06980238672441;9.06905841494425;0.0940528320212791]
Robot Arm 1 togoal: 27.005972119542

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958683e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-0.399426375306949;-9.1821693673805;-0.512745014439203]
Robot Arm 1 end: [-9.57068324638341;-0.415488266636346;0.117239903969179]
Robot Arm 1 togoal: 22.2140626849282
Mobile Platform 2 Pose: [-1.37960166530695;9.35554979972374;9.38061099185736]
Robot Arm 2 end: [9.04425414582226;9.011872176659;0.100886221145219]
Robot Arm 1 togoal: 26.9472006482525

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949195e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-0.401419403203395;-9.18081814759248;-0.438807674038463]
Robot Arm 1 end: [-9.56975525040709;-0.340618964751715;0.117125555860492]
Robot Arm 1 togoal: 22.1782457509589
Mobile Platform 2 Pose: [-1.36261500714025;9.34449501759488;9.33144480662736]
Robot Arm 2 end: [9.02017020069238;8.95271276328136;0.107769953785064]
Robot Arm 1 togoal: 26.8880905739749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.936606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-0.403429530140673;-9.17945527397169;-0.364178159012197]
Robot Arm 1 end: [-9.56881620035399;-0.265056538084259;0.117013580640775]
Robot Arm 1 togoal: 22.1422928144094
Mobile Platform 2 Pose: [-1.34556727005035;9.33334437492777;9.27871219967728]
Robot Arm 2 end: [8.99742753906;8.89165174894693;0.114477278853357]
Robot Arm 1 togoal: 26.8286239876447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-0.405456722642464;-9.17808076952708;-0.288846412231139]
Robot Arm 1 end: [-9.56786610856222;-0.188790922068029;0.116903986240487]
Robot Arm 1 togoal: 22.106205692341
Mobile Platform 2 Pose: [-1.32193234172583;9.3188449380004;9.22270152237517]
Robot Arm 2 end: [8.97261254104746;8.82879217298218;0.120846996547911]
Robot Arm 1 togoal: 26.7664691313431

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-0.407500945360967;-9.17669465854564;-0.212802224641507]
Robot Arm 1 end: [-9.56690498835655;-0.111811900037077;0.116796780538786]
Robot Arm 1 togoal: 22.0699863481301
Mobile Platform 2 Pose: [-1.29873629100216;9.30451844521542;9.1636729139465]
Robot Arm 2 end: [8.94917020115369;8.76437551635727;0.12691215123984]
Robot Arm 1 togoal: 26.7042378801796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-0.409562160994018;-9.17529696664887;-0.136035233061095]
Robot Arm 1 end: [-9.56593285409524;-0.0341091010192317;0.116691971361658]
Robot Arm 1 togoal: 22.0336368977393
Mobile Platform 2 Pose: [-1.27603009079148;9.29039564993723;9.10185921213322]
Robot Arm 2 end: [8.92707477238768;8.69867646202087;0.13273924420875]
Robot Arm 1 togoal: 26.642108189799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.878654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-0.411640330198978;-9.17388772085158;-0.0585349179780993]
Robot Arm 1 end: [-9.56494972121861;0.0443280024672959;0.116589566479948]
Robot Arm 1 togoal: 21.9971596162334
Mobile Platform 2 Pose: [-1.25384606556946;9.27649495741423;9.03743606236696]
Robot Arm 2 end: [8.90622301201036;8.63174795975114;0.138220636718343]
Robot Arm 1 togoal: 26.5800560455218

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.256825e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-0.413735411503273;-9.17246694962283;0.0197093986441526]
Robot Arm 1 end: [-9.56395560629965;0.123510096611311;0.116489573607277]
Robot Arm 1 togoal: 21.9605569445506
Mobile Platform 2 Pose: [-1.23184949091138;9.26265140995877;8.97059401951206]
Robot Arm 2 end: [8.8831531682706;8.56099775439704;0.145605729830279]
Robot Arm 1 togoal: 26.5136911917884

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-0.415847361211464;-9.17103468294933;0.098708555553128]
Robot Arm 1 end: [-9.5629505270966;0.203448028496578;0.116392000397845]
Robot Arm 1 togoal: 21.9238314965369
Mobile Platform 2 Pose: [-1.07774361564428;9.18271266507534;8.90053701870344]
Robot Arm 2 end: [8.80072865032773;8.49026189426562;0.143839258533444]
Robot Arm 1 togoal: 26.4055629443801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-0.417997889247062;-9.16958007558546;0.178473561851782]
Robot Arm 1 end: [-9.56498098925925;0.291539163097052;0.119041880780423]
Robot Arm 1 togoal: 21.8862554928841
Mobile Platform 2 Pose: [-1.0621428771881;9.17204859979447;8.83028108497553]
Robot Arm 2 end: [8.78751253015128;8.42236757304963;0.143821955316219]
Robot Arm 1 togoal: 26.3486408270693

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-0.420015611376131;-9.16821760809262;0.255310482928022]
Robot Arm 1 end: [-9.56648730254294;0.37804082863991;0.121852359958562]
Robot Arm 1 togoal: 21.849190780771
Mobile Platform 2 Pose: [-1.04652415335726;9.16137237094195;8.75961080386745]
Robot Arm 2 end: [8.77430404165846;8.3540745082473;0.143802749794986]
Robot Arm 1 togoal: 26.2915046273297

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-0.421906524324637;-9.16694438140314;0.328709020867258]
Robot Arm 1 end: [-9.56777656407498;0.464421522223602;0.125592200841548]
Robot Arm 1 togoal: 21.8122052872548
Mobile Platform 2 Pose: [-1.03088761033472;9.15068409328359;8.68852726839002]
Robot Arm 2 end: [8.76110339965145;8.28538359002773;0.143781642189764]
Robot Arm 1 togoal: 26.2341568229657

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978902e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-0.423644936096636;-9.16577797157959;0.397306067778096]
Robot Arm 1 end: [-9.56806480889697;0.548823907524004;0.130033053229741]
Robot Arm 1 togoal: 21.7754145527287
Mobile Platform 2 Pose: [-1.01523341177761;9.13998387990535;8.61703158032193]
Robot Arm 2 end: [8.7479108098737;8.21629571934107;0.143758634479535]
Robot Arm 1 togoal: 26.1765999176433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980905e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-0.425228386196237;-9.16471864817995;0.460224922702221]
Robot Arm 1 end: [-9.5666584874321;0.626330321965005;0.134322778325128]
Robot Arm 1 togoal: 21.7403468535767
Mobile Platform 2 Pose: [-0.999561719972543;9.12927184281742;8.5451248457828]
Robot Arm 2 end: [8.73472647667022;8.14681179834045;0.143733729666335]
Robot Arm 1 togoal: 26.1188364399993

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-0.426712549721577;-9.16372855815456;0.518089436627684]
Robot Arm 1 end: [-9.56339303723499;0.699047178490778;0.138183174582205]
Robot Arm 1 togoal: 21.7059036414044
Mobile Platform 2 Pose: [-0.983872694814786;9.11854809247751;8.47280817169549]
Robot Arm 2 end: [8.72155059455297;8.0769327317241;0.143706933248449]
Robot Arm 1 togoal: 26.0608689387635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986568e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-0.428139871007609;-9.16277757500949;0.571230756121294]
Robot Arm 1 end: [-9.55857186315042;0.764399607409091;0.141160948914228]
Robot Arm 1 togoal: 21.6734400533129
Mobile Platform 2 Pose: [-0.968166494949845;9.10781273838504;8.40008266132178]
Robot Arm 2 end: [8.70838335593411;8.00665941882636;0.143678251476243]
Robot Arm 1 togoal: 26.0026999832163

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-0.429560526710562;-9.16183025608708;0.620491199659417]
Robot Arm 1 end: [-9.55302414704309;0.820288023130805;0.143063589956575]
Robot Arm 1 togoal: 21.6445506773241
Mobile Platform 2 Pose: [-0.952443277084898;9.09706588868348;8.32694941057996]
Robot Arm 2 end: [8.69522494428243;7.93599275300941;0.143647693075026]
Robot Arm 1 togoal: 25.9443321581173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994309e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-0.431017478306814;-9.1608566736392;0.666829318320266]
Robot Arm 1 end: [-9.54737423261202;0.866448798094512;0.1439635895983]
Robot Arm 1 togoal: 21.6198479694744
Mobile Platform 2 Pose: [-0.936703196596606;9.08630765060883;8.25340950385178]
Robot Arm 2 end: [8.68207554099632;7.86493361463228;0.143615267468593]
Robot Arm 1 togoal: 25.8857680642496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995968e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-0.432544100134878;-9.15983473822969;0.710962005617648]
Robot Arm 1 end: [-9.54158016358481;0.906150067326057;0.144314388118907]
Robot Arm 1 togoal: 21.5978407523013
Mobile Platform 2 Pose: [-0.920946407375242;9.07553813032803;8.17946401027389]
Robot Arm 2 end: [8.6689353192155;7.79348286999054;0.143580986590652]
Robot Arm 1 togoal: 25.8270103135497

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996698e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-0.43415681343095;-9.15875345302666;0.753240247422742]
Robot Arm 1 end: [-9.53576239163648;0.940768626901987;0.144405407049426]
Robot Arm 1 togoal: 21.5780142266878
Mobile Platform 2 Pose: [-0.905173062374633;9.06475743334001;8.10511397978325]
Robot Arm 2 end: [8.65580444967089;7.72164136501591;0.143544863289976]
Robot Arm 1 togoal: 25.768061529503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996914e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-0.435868549067397;-9.15760425675172;0.793884054799446]
Robot Arm 1 end: [-9.52954619873915;0.97171229762968;0.144322515828041]
Robot Arm 1 togoal: 21.559415448033
Mobile Platform 2 Pose: [-0.889383312872868;9.05396566398959;8.03036043951748]
Robot Arm 2 end: [8.64268309811695;7.64940992264334;0.143506911910555]
Robot Arm 1 togoal: 25.7089243439326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996852e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-0.4376957135582;-9.15637627699117;0.832980917623195]
Robot Arm 1 end: [-9.52248233628788;1.00023687829429;0.144053722264847]
Robot Arm 1 togoal: 21.5411010366057
Mobile Platform 2 Pose: [-0.873577309473551;9.0431629261632;7.955204390071]
Robot Arm 2 end: [8.62957142335116;7.57678934107612;0.143467148794818]
Robot Arm 1 togoal: 25.6496013948455

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-0.439652607392304;-9.15505887247737;0.870507794102825]
Robot Arm 1 end: [-9.51466976840029;1.02427179462672;0.143388413119331]
Robot Arm 1 togoal: 21.5240295175127
Mobile Platform 2 Pose: [-0.857755201991054;9.03234932309655;7.87964680137389]
Robot Arm 2 end: [8.61646958089056;7.5037803870395;0.143425591586327]
Robot Arm 1 togoal: 25.5900953250403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995156e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-0.441771222955848;-9.15363016167779;0.906595176660352]
Robot Arm 1 end: [-9.50585850339483;1.044042092098;0.142436575620343]
Robot Arm 1 togoal: 21.507862169858
Mobile Platform 2 Pose: [-0.841917139486122;9.02152495751506;7.80368860922432]
Robot Arm 2 end: [8.60337772059869;7.43038379425269;0.143382259644245]
Robot Arm 1 togoal: 25.5304087798993

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-0.44409725725787;-9.15205910337893;0.94146463447977]
Robot Arm 1 end: [-9.49570732907928;1.06092212350786;0.141421062439355]
Robot Arm 1 togoal: 21.4916950375736
Mobile Platform 2 Pose: [-0.826063270487383;9.01068993171939;7.72733071181445]
Robot Arm 2 end: [8.59029599067472;7.35660025709166;0.143337173375868]
Robot Arm 1 togoal: 25.4705444066128

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-0.446710557010267;-9.15029718131058;0.975494655056794]
Robot Arm 1 end: [-9.49372872801548;1.09655549807128;0.141138456453106]
Robot Arm 1 togoal: 21.475120525849
Mobile Platform 2 Pose: [-0.810193743081588;8.99984434775243;7.65057396636417]
Robot Arm 2 end: [8.57722453403954;7.28243043013427;0.143290354866192]
Robot Arm 1 togoal: 25.4105048518688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-0.449323431918308;-9.14853558472198;1.00956687721437]
Robot Arm 1 end: [-9.49186488541987;1.1322035593915;0.14062872326957]
Robot Arm 1 togoal: 21.4587053496979
Mobile Platform 2 Pose: [-0.794308705055866;8.98898830737217;7.57341918550886]
Robot Arm 2 end: [8.56416348966919;7.20787492361744;0.143241827639959]
Robot Arm 1 togoal: 25.3502927604305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-0.451910471143264;-9.14678563988224;1.04366016663698]
Robot Arm 1 end: [-9.48251691512246;1.14928766133389;0.138916559889415]
Robot Arm 1 togoal: 21.4432676820786
Mobile Platform 2 Pose: [-0.77840830396752;8.97812191217537;7.49586713397313]
Robot Arm 2 end: [8.55111299343675;7.1329342998725;0.143191616400663]
Robot Arm 1 togoal: 25.2899107740799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-0.45480139145551;-9.14483446228768;1.07808787774256]
Robot Arm 1 end: [-9.48055585308873;1.18526620686895;0.138399367832211]
Robot Arm 1 togoal: 21.4266929948872
Mobile Platform 2 Pose: [-0.762492687901535;8.96724526405183;7.41791852524324]
Robot Arm 2 end: [8.5380731766854;7.05760907079955;0.143139747728048]
Robot Arm 1 togoal: 25.2293615295979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-0.457698853516038;-9.14287885552138;1.11257600118453]
Robot Arm 1 end: [-9.47858632290191;1.2213098508771;0.137884697978505]
Robot Arm 1 togoal: 21.4101315692299
Mobile Platform 2 Pose: [-0.74656200511615;8.95635846501474;7.33957401818946]
Robot Arm 2 end: [8.52504416744937;6.98189969400038;0.143086249570423]
Robot Arm 1 togoal: 25.1686476578831

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-0.460602722657801;-9.14091891171023;1.14712554247716]
Robot Arm 1 end: [-9.47660836986249;1.25741960130339;0.137372575389433]
Robot Arm 1 togoal: 21.3935833948127
Mobile Platform 2 Pose: [-0.730616403861429;8.94546161699139;7.26083421386478]
Robot Arm 2 end: [8.5120260922266;6.90580656878004;0.143031150509564]
Robot Arm 1 togoal: 25.1077717834628

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-0.463512863266756;-9.13895472362894;1.18173750851036]
Robot Arm 1 end: [-9.47462203954718;1.29359646711955;0.136863025183777]
Robot Arm 1 togoal: 21.377048468524
Mobile Platform 2 Pose: [-0.714656033638598;8.93455482276562;7.18169965261636]
Robot Arm 2 end: [8.4990190723886;6.82933003531092;0.14297448138177]
Robot Arm 1 togoal: 25.0467365221096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-0.466429138769479;-9.13698638470845;1.21641290754394]
Robot Arm 1 end: [-9.47262737781637;1.32984145831318;0.136356072535993]
Robot Arm 1 togoal: 21.3605267945136
Mobile Platform 2 Pose: [-0.698681044141314;8.92363818524241;7.10217081064634]
Robot Arm 2 end: [8.48602322833679;6.75247036882285;0.142916273651561]
Robot Arm 1 togoal: 24.9855444810339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-0.469351411620711;-9.13501398904445;1.2511527492002]
Robot Arm 1 end: [-9.47062443082173;1.36615558587516;0.1358517426742]
Robot Arm 1 togoal: 21.3440183842734
Mobile Platform 2 Pose: [-0.682691586111952;8.91271180802028;7.02224809737411]
Robot Arm 2 end: [8.47303867724418;6.67522777854892;0.142856560272044]
Robot Arm 1 togoal: 24.9241982574749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-0.472279543290836;-9.13303763140592;1.28595804445463]
Robot Arm 1 end: [-9.46861324501394;1.40253986178507;0.135350060878135]
Robot Arm 1 togoal: 21.3275232567196
Mobile Platform 2 Pose: [-0.666687811674597;8.90177579556255;6.94193185259704]
Robot Arm 2 end: [8.46006553488941;6.59760240384083;0.142795375247839]
Robot Arm 1 togoal: 24.8627004385301

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-0.475213394253303;-9.13105740724366;1.32082980562477]
Robot Arm 1 end: [-9.46659386715034;1.43899529899482;0.134851052477079]
Robot Arm 1 togoal: 21.3110414382762
Mobile Platform 2 Pose: [-0.650669874217814;8.89083025318743;6.86122234330535]
Robot Arm 2 end: [8.44710391234573;6.51959431345416;0.142732754386657]
Robot Arm 1 togoal: 24.8010535992792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-0.478152823971977;-9.12907341269898;1.3557690463572]
Robot Arm 1 end: [-9.46456634430288;1.47552291141015;0.13435474284775]
Robot Arm 1 togoal: 21.2945729629604
Mobile Platform 2 Pose: [-0.634637928477527;8.87987528710149;6.78011976090652]
Robot Arm 2 end: [8.43415392187596;6.44120349861574;0.142668733887541]
Robot Arm 1 togoal: 24.7392603037652

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-0.481097690888312;-9.12708574461234;1.39077678161253]
Robot Arm 1 end: [-9.462530723866;1.51212371387015;0.133861157412161]
Robot Arm 1 togoal: 21.2781178724691
Mobile Platform 2 Pose: [-0.618592131296567;8.86891100491651;6.69862421883443]
Robot Arm 2 end: [8.42121567122346;6.36242987492033;0.142603351950934]
Robot Arm 1 togoal: 24.67732310314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-0.484047852408538;-9.12509450053205;1.42585402764834]
Robot Arm 1 end: [-9.46048705356466;1.54879872212467;0.133370321635457]
Robot Arm 1 togoal: 21.2616762162676
Mobile Platform 2 Pose: [-0.602532640899477;8.85793751518695;6.61673574945023]
Robot Arm 2 end: [8.40828926843141;6.28327327587779;0.142536647267057]
Robot Arm 1 togoal: 24.6152445363029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-0.487003164891324;-9.12309977872294;1.46100180200028]
Robot Arm 1 end: [-9.45843538146239;1.58554895280972;0.132882261023702]
Robot Arm 1 togoal: 21.245248051679
Mobile Platform 2 Pose: [-0.586459617670682;8.84695492792052;6.53445430194084]
Robot Arm 2 end: [8.39537482021231;6.20373345215985;0.14246865963312]
Robot Arm 1 togoal: 24.5530271294828

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-0.489963483634618;-9.12110167817517;1.4962211234608]
Robot Arm 1 end: [-9.45637575596951;1.62237542342048;0.132397001121649]
Robot Arm 1 togoal: 21.2288334439762
Mobile Platform 2 Pose: [-0.570373224495806;8.83596335474802;6.45177973977386]
Robot Arm 2 end: [8.38247243087186;6.12381006981305;0.142399430365759]
Robot Arm 1 togoal: 24.4906733955962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-0.492928662862722;-9.11910029861304;1.53151301205573]
Robot Arm 1 end: [-9.45430822585138;1.65927915228207;0.131914567510462]
Robot Arm 1 togoal: 21.2124324664745
Mobile Platform 2 Pose: [-0.554273626823685;8.82496290906875;6.36871183785986]
Robot Arm 2 end: [8.36958220424745;6.04350270671001;0.1423290015638]
Robot Arm 1 togoal: 24.428185834825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-0.495898555713339;-9.11709574050379;1.5668784890187]
Robot Arm 1 end: [-9.45223284023684;1.69626115851802;0.131434985805417]
Robot Arm 1 togoal: 21.1960452006264
Mobile Platform 2 Pose: [-0.53816099261721;8.81395370595793;6.28525028039973]
Robot Arm 2 end: [8.35670424289975;5.96281085028833;0.142257416601491]
Robot Arm 1 togoal: 24.3655669339881

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-0.498873014224559;-9.11508810506652;1.6023185767632]
Robot Arm 1 end: [-9.45014964862658;1.73332246201634;0.130958281653549]
Robot Arm 1 togoal: 21.179671736118
Mobile Platform 2 Pose: [-0.522035492714782;8.80293586238405;6.20139465847957]
Robot Arm 2 end: [8.34383865006866;5.88173389440018;0.142184719393992]
Robot Arm 1 togoal: 24.3028191675436

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-0.50185188932182;-9.11307749428103;1.63783429885232]
Robot Arm 1 end: [-9.44805870090176;1.77046408339314;0.130484480731284]
Robot Arm 1 togoal: 21.1633121709663
Mobile Platform 2 Pose: [-0.505897300985832;8.79190949743199;6.11714446818168]
Robot Arm 2 end: [8.33098552794382;5.8002711386464;0.14211095502346]
Robot Arm 1 togoal: 24.2399449974373

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-0.504835030804822;-9.11106401089676;1.67342667996602]
Robot Arm 1 end: [-9.44596004733261;1.80768704395374;0.130013608742018]
Robot Arm 1 togoal: 21.1469666116189
Mobile Platform 2 Pose: [-0.48974659451824;8.78087473234828;6.03249910823374]
Robot Arm 2 end: [8.318144979166;5.71842178505575;0.142036169281189]
Robot Arm 1 togoal: 24.1769468737699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-0.507822287334408;-9.10904775844171;1.70909674586592]
Robot Arm 1 end: [-9.44385373858713;1.84499236565116;0.129545691413664]
Robot Arm 1 togoal: 21.1306351730548
Mobile Platform 2 Pose: [-0.473583554254463;8.7698316909451;5.9474578779748]
Robot Arm 2 end: [8.30531710452771;5.6361849379627;0.141960409485099]
Robot Arm 1 togoal: 24.1138272346738

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-0.510813506419539;-9.10702884123135;1.74484552335756]
Robot Arm 1 end: [-9.44173982573992;1.88238107104197;0.129080754496161]
Robot Arm 1 togoal: 21.1143179788868
Mobile Platform 2 Pose: [-0.457408364396024;8.7587804992254;5.86201997502559]
Robot Arm 2 end: [8.29250200758708;5.55355959809715;0.141883722971149]
Robot Arm 1 togoal: 24.0505885078661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-0.513808534404253;-9.10500736437764;1.7806740402499]
Robot Arm 1 end: [-9.43961836028106;1.91985418323937;0.128618823758943]
Robot Arm 1 togoal: 21.0980151614658
Mobile Platform 2 Pose: [-0.441221213586926;8.74772128625362;5.77618449374726]
Robot Arm 2 end: [8.27969978967679;5.47054466559932;0.141806158714897]
Robot Arm 1 togoal: 23.9872331106122

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-0.515811349289495;-9.10348136604565;1.81658332288825]
Robot Arm 1 end: [-9.43545863388718;1.95461007675408;0.126954513492812]
Robot Arm 1 togoal: 21.0810036323003
Mobile Platform 2 Pose: [-0.425022294198683;8.7366541835372;5.68995042281291]
Robot Arm 2 end: [8.26691055475943;5.38713893310885;0.141727765880572]
Robot Arm 1 togoal: 23.9237634509746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-0.51681422087053;-9.10245676112081;1.85235377475571]
Robot Arm 1 end: [-9.43478291901306;1.99284533880117;0.127347246491115]
Robot Arm 1 togoal: 21.0657917220047
Mobile Platform 2 Pose: [-0.408811802836053;8.7255793255479;5.60331664389225]
Robot Arm 2 end: [8.25413440734189;5.30334108626628;0.141648594602294]
Robot Arm 1 togoal: 23.8601819285495

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-0.51803571264052;-9.1013263553321;1.88831895869775]
Robot Arm 1 end: [-9.43477990028051;2.03223471897082;0.128278569848481]
Robot Arm 1 togoal: 21.0507868339386
Mobile Platform 2 Pose: [-0.39258994111861;8.71449685005435;5.51628192976996]
Robot Arm 2 end: [8.24137145230105;5.21914970385679;0.141568695500476]
Robot Arm 1 togoal: 23.7964909365448

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-0.520580601763798;-9.09953818545076;1.92456884544117]
Robot Arm 1 end: [-9.43370007874411;2.07035944809224;0.126796759055179]
Robot Arm 1 togoal: 21.0354867001004
Mobile Platform 2 Pose: [-0.376356914998499;8.70340689786245;5.42884494236074]
Robot Arm 2 end: [8.22862179503617;5.13456325297148;0.141488120549692]
Robot Arm 1 togoal: 23.7326928609083

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-0.52459161205312;-9.09700615535488;1.96078442229646]
Robot Arm 1 end: [-9.43103971017017;2.10824866274312;0.126350273939024]
Robot Arm 1 togoal: 21.0188032781533
Mobile Platform 2 Pose: [-0.36011293555573;8.69230961316046;5.34100423117866]
Robot Arm 2 end: [8.21588554394036;5.04958008748947;0.141406921746003]
Robot Arm 1 togoal: 23.6687900835945

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-0.528361545454063;-9.09459412752092;1.9970905422941]
Robot Arm 1 end: [-9.428494616286;2.14623163612796;0.125906947223943]
Robot Arm 1 togoal: 21.0022466701616
Mobile Platform 2 Pose: [-0.343858219168037;8.68120514377314;5.25275823193887]
Robot Arm 2 end: [8.20316280887959;4.96419844785928;0.141325151696591]
Robot Arm 1 togoal: 23.6047849835692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-0.531377819576695;-9.09255843497679;2.03348710643178]
Robot Arm 1 end: [-9.42632243942915;2.18430527478818;0.125466805465183]
Robot Arm 1 togoal: 20.9860572870518
Mobile Platform 2 Pose: [-0.327592987602456;8.67009364117796;5.16410526490211]
Robot Arm 2 end: [8.19045369961335;4.87841646040809;0.141242864238626]
Robot Arm 1 togoal: 23.540679937521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-0.534396628413022;-9.09052105438277;2.06997158890203]
Robot Arm 1 end: [-9.42414313926031;2.22247144343246;0.125029876522784]
Robot Arm 1 togoal: 20.9698836364997
Mobile Platform 2 Pose: [-0.311317468356944;8.65897526071504;5.07504353305259]
Robot Arm 2 end: [8.17775832977684;4.79223213280107;0.141160113127395]
Robot Arm 1 togoal: 23.4764773226276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-0.537417807404702;-9.08848209830175;2.10654502317034]
Robot Arm 1 end: [-9.421956771332;2.26073116647771;0.124596186218342]
Robot Arm 1 togoal: 20.9537259339756
Mobile Platform 2 Pose: [-0.295031894539754;8.64785016157714;4.98557112115715]
Robot Arm 2 end: [8.16507681522299;4.70564335403619;0.141076952654212]
Robot Arm 1 togoal: 23.4121795178419

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-0.540441190698185;-9.0864416801816;2.14320844313834]
Robot Arm 1 end: [-9.41976339170539;2.29908546825962;0.124165760365197]
Robot Arm 1 togoal: 20.9375844047972
Mobile Platform 2 Pose: [-0.278736505333666;8.63671850707692;4.89568599443623]
Robot Arm 2 end: [8.15240927221386;4.61864789498657;0.140993438224927]
Robot Arm 1 togoal: 23.3477889054637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-0.543466611131559;-9.08439991436414;2.17996288306983]
Robot Arm 1 end: [-9.41756305696014;2.33753537295095;0.123738624765414]
Robot Arm 1 togoal: 20.9214592842498
Mobile Platform 2 Pose: [-0.262431546667446;8.62558046508509;4.80538599674754]
Robot Arm 2 end: [8.13975582146261;4.53124340400116;0.14090962505537]
Robot Arm 1 togoal: 23.283307874347

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-0.546493900221768;-9.0823569160938;2.21680937751322]
Robot Arm 1 end: [-9.41535582420414;2.37608190447624;0.123314805206707]
Robot Arm 1 togoal: 20.9053508177083
Mobile Platform 2 Pose: [-0.246117270816138;8.61443620776071;4.71466884972818]
Robot Arm 2 end: [8.12711658654255;4.44342740646043;0.14082556879431]
Robot Arm 1 togoal: 23.218738821227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-0.54952288815111;-9.08031280152683;2.2537489612205]
Robot Arm 1 end: [-9.41314175108391;2.41472608642291;0.122894327459321]
Robot Arm 1 togoal: 20.8892592607603
Mobile Platform 2 Pose: [-0.229793936558906;8.60328591175494;4.62353215172465]
Robot Arm 2 end: [8.11449169257365;4.3551973052504;0.140741325890454]
Robot Arm 1 togoal: 23.1540841529187

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-0.552553403754057;-9.07826768774029;2.29078266906248]
Robot Arm 1 end: [-9.41092089579487;2.45346894194849;0.122477217272853]
Robot Arm 1 togoal: 20.8731848793314
Mobile Platform 2 Pose: [-0.2134618100705;8.59212975868333;4.53197337634871]
Robot Arm 2 end: [8.10188126781664;4.26655037861013;0.140656953133855]
Robot Arm 1 togoal: 23.0893462892864

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-0.555585274504513;-9.07622169274069;2.32791153594012]
Robot Arm 1 end: [-9.40869331709141;2.49231149368382;0.122063500373025]
Robot Arm 1 togoal: 20.8571279498106
Mobile Platform 2 Pose: [-0.197121164622154;8.58096793500378;4.43998987126231]
Robot Arm 2 end: [8.08928544501594;4.17748377744981;0.140572507212496]
Robot Arm 1 togoal: 23.0245276658168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-0.558618326502882;-9.07417493547278;2.36513659669204]
Robot Arm 1 end: [-9.40645907429718;2.53125476363233;0.1216532024584]
Robot Arm 1 togoal: 20.8410887591778
Mobile Platform 2 Pose: [-0.180772281046256;8.5698006323231;4.34757885743345]
Robot Arm 2 end: [8.07670436065349;4.08799452524403;0.140488045097021]
Robot Arm 1 togoal: 22.9596307362162

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-0.561652384463213;-9.07212753582827;2.40245888599806]
Robot Arm 1 end: [-9.40421822731558;2.57029977306524;0.121246349197043]
Robot Arm 1 togoal: 20.8250676051324
Mobile Platform 2 Pose: [-0.164415447854872;8.55862804742798;4.25473742840918]
Robot Arm 2 end: [8.06413815359099;3.99807951868809;0.140403624415703]
Robot Arm 1 togoal: 22.8946579750782

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-0.564687271700412;-9.07007961465454;2.43987943827872]
Robot Arm 1 end: [-9.40197083664018;2.60944754241257;0.120842966223123]
Robot Arm 1 togoal: 20.8090647962234
Mobile Platform 2 Pose: [-0.148050961501333;8.54745038257321;4.1614625488538]
Robot Arm 2 end: [8.05158696695866;3.90773552514742;0.140319302797772]
Robot Arm 1 togoal: 22.8296118812989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-0.567722810117537;-9.06803129376328;2.4773992875906]
Robot Arm 1 end: [-9.39971696336539;2.64869909114984;0.120443079133456]
Robot Arm 1 togoal: 20.7930806519798
Mobile Platform 2 Pose: [-0.131679126568234;8.53626784553325;4.06775105376453]
Robot Arm 2 end: [8.03905094758134;3.81695918180114;0.140235138249484]
Robot Arm 1 togoal: 22.7644949806588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986618e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-0.570710972711997;-9.06600660876193;2.51501949687451]
Robot Arm 1 end: [-9.38788925794278;2.66441141544922;0.116030701056749]
Robot Arm 1 togoal: 20.7767911018766
Mobile Platform 2 Pose: [-0.115300256330284;8.52508064995393;3.97359964775207]
Robot Arm 2 end: [8.02653024820192;3.72574699314592;0.140151188497042]
Robot Arm 1 togoal: 22.6993098298037

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-0.574178473446981;-9.0636633478511;2.55627780075293]
Robot Arm 1 end: [-9.38550789694522;2.70735495218592;0.115630396605513]
Robot Arm 1 togoal: 20.7594713790616
Mobile Platform 2 Pose: [-0.0989146728107988;8.51388901543701;3.87900490457196]
Robot Arm 2 end: [8.01402502484487;3.63409533456221;0.140067511375755]
Robot Arm 1 togoal: 22.6340590201096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-0.577645034876141;-9.06132076742708;2.59772200671701]
Robot Arm 1 end: [-9.3831218312083;2.75048926890407;0.11523369268014]
Robot Arm 1 togoal: 20.7421550634097
Mobile Platform 2 Pose: [-0.0825227066715875;8.50269316752696;3.78396326601482]
Robot Arm 2 end: [8.00153543723723;3.54200044844722;0.139984165409527]
Robot Arm 1 togoal: 22.5687451795451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-0.581110438708517;-9.05897901673594;2.63935388332245]
Robot Arm 1 end: [-9.38073114562583;2.79381612811935;0.114840613890567]
Robot Arm 1 togoal: 20.7248428209607
Mobile Platform 2 Pose: [-0.0661246981064286;8.49149333819076;3.68847104107263]
Robot Arm 2 end: [7.98906165173738;3.44945844235964;0.139901208523605]
Robot Arm 1 togoal: 22.5033709780243

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-0.584574463863872;-9.05663824692817;2.6811752079071]
Robot Arm 1 end: [-9.37833592639673;2.83733730069413;0.114451184775976]
Robot Arm 1 togoal: 20.7075353383717
Mobile Platform 2 Pose: [-0.0497209966976868;8.48028976580795;3.59252440588057]
Robot Arm 2 end: [7.9766038390892;3.3564652907826;0.139818698843972]
Robot Arm 1 togoal: 22.4379391308137

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-0.588036886435773;-9.05429861108385;2.72318776650476]
Robot Arm 1 end: [-9.37593626104665;2.88105456574299;0.114065429801489]
Robot Arm 1 togoal: 20.6902333233072
Mobile Platform 2 Pose: [-0.033311961857804;8.46908269545948;3.49611940294548]
Robot Arm 2 end: [7.96416217513962;3.26301683412922;0.139736694467161]
Robot Arm 1 togoal: 22.3724524028173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-0.591497479654437;-9.05196026423798;2.76539335375195]
Robot Arm 1 end: [-9.37353223844989;2.92496971053089;0.113683373354805]
Robot Arm 1 togoal: 20.6729375048345
Mobile Platform 2 Pose: [-0.0168979629577996;8.45787237896657;3.39925194049525]
Robot Arm 2 end: [7.9517368411708;3.16910877748391;0.139655253408755]
Robot Arm 1 togoal: 22.306913612528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-0.594956013849371;-9.0496233634059;2.80779377278733]
Robot Arm 1 end: [-9.37112394885146;2.96908453036383;0.11330503974279]
Robot Arm 1 togoal: 20.6556486338258
Mobile Platform 2 Pose: [-0.000479379602554314;8.44665907518017;3.30191779210413]
Robot Arm 2 end: [7.93932802440686;3.07473668968412;0.139574433572785]
Robot Arm 1 togoal: 22.2413256364683

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-0.59841225641179;-9.04728806760891;2.85039083514369]
Robot Arm 1 end: [-9.36871148388945;3.01340082847179;0.112930453188012]
Robot Arm 1 togoal: 20.6383674833648
Mobile Platform 2 Pose: [0.0159433980848476;8.4354430501083;3.20411259634461]
Robot Arm 2 end: [7.92693591693604;2.97989600561677;0.139494292716058]
Robot Arm 1 togoal: 22.1756914143802

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-0.601865971756845;-9.04495453790001;2.89318636063191]
Robot Arm 1 end: [-9.36629493661756;3.05792041588361;0.112559637825212]
Robot Arm 1 togoal: 20.6210948491604
Mobile Platform 2 Pose: [0.0323699689234205;8.42422457725085;3.10583185644488]
Robot Arm 2 end: [7.91456071834933;2.88458202140386;0.13941488821784]
Robot Arm 1 togoal: 22.1100139533834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-0.605316921285656;-9.04262293738974;2.93618217721708]
Robot Arm 1 end: [-9.3638744015279;3.10264511129348;0.112192617697724]
Robot Arm 1 togoal: 20.6038315499652
Mobile Platform 2 Pose: [0.0487999206998878;8.41300393764041;3.00707093998842]
Robot Arm 2 end: [7.90220263258117;2.78878989842012;0.139336277834538]
Robot Arm 1 togoal: 22.0442963327269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-0.608764863347182;-9.04029343127216;2.97938012088606]
Robot Arm 1 end: [-9.36144997457393;3.14757674091896;0.111829416753833]
Robot Arm 1 togoal: 20.5865784280004
Mobile Platform 2 Pose: [0.0652328296894315;8.40178142013499;2.90782507855959]
Robot Arm 2 end: [7.88986187172218;2.69251465922358;0.139258518450417]
Robot Arm 1 togoal: 21.9785417097163

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-0.612209553199917;-9.03796618685096;3.02278203550655]
Robot Arm 1 end: [-9.35902175319372;3.19271713835008;0.111470058843069]
Robot Arm 1 togoal: 20.5693363493857
Mobile Platform 2 Pose: [0.0816682606622316;8.39055732147165;2.80808936798818]
Robot Arm 2 end: [7.87753865417758;2.5957511897902;0.139181666683034]
Robot Arm 1 togoal: 21.912753324788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-0.615650742973449;-9.03564137356558;3.06638977267719]
Robot Arm 1 end: [-9.35658983633332;3.23806814438923;0.111114567712437]
Robot Arm 1 togoal: 20.5521062045753
Mobile Platform 2 Pose: [0.0981057663216967;8.37933194661318;2.70785876804289]
Robot Arm 2 end: [7.86523320410218;2.49849423991403;0.139105779234981]
Robot Arm 1 togoal: 21.8469345066877

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-0.61908818162988;-9.03331916301755;3.1102051915684]
Robot Arm 1 end: [-9.35415432447042;3.28363160688166;0.110762967002588]
Robot Arm 1 togoal: 20.5348889087994
Mobile Platform 2 Pose: [0.114544887021236;8.36810560887972;2.60712810214372]
Robot Arm 2 end: [7.85294575338288;2.40073842079034;0.139030912217756]
Robot Arm 1 togoal: 21.7810886783394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-0.622521614925134;-9.03099972899679;3.15423015875384]
Robot Arm 1 end: [-9.35171531963814;3.3294093805362;0.11041528024391]
Robot Arm 1 togoal: 20.5176854025108
Mobile Platform 2 Pose: [0.130985150419722;8.35687863028392;2.50589205766836]
Robot Arm 2 end: [7.84067654113801;2.30247820639333;0.138957121357048]
Robot Arm 1 togoal: 21.7152193629883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-0.625950785370162;-9.02868324750804;3.19846654803188]
Robot Arm 1 end: [-9.3492729254491;3.37540332673586;0.110071530852561]
Robot Arm 1 togoal: 20.5004966518373
Mobile Platform 2 Pose: [0.147426071322774;8.34565134158913;2.40414518612135]
Robot Arm 2 end: [7.82842581386293;2.20370793339628;0.138884461945182]
Robot Arm 1 togoal: 21.6493301902065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-0.629375432192065;-9.02636989679733;3.24291624023712]
Robot Arm 1 end: [-9.34682724711963;3.42161531333809;0.109731742126421]
Robot Arm 1 togoal: 20.483323649039
Mobile Platform 2 Pose: [0.163867151229949;8.33442408263571;2.30188190325228]
Robot Arm 2 end: [7.81619382583766;2.10442180104187;0.13881298878963]
Robot Arm 1 togoal: 21.5834249022557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-0.631377327536195;-9.02476883890736;3.28758112337398]
Robot Arm 1 end: [-9.34207527375014;3.46446876731729;0.107764352445169]
Robot Arm 1 togoal: 20.4652456468108
Mobile Platform 2 Pose: [0.18030787821844;8.3231972023911;2.19909648931562]
Robot Arm 2 end: [7.80398083881897;2.00461387209972;0.138742756239518]
Robot Arm 1 togoal: 21.5175073606394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-0.629459808339413;-9.02513341592851;3.33219852797934]
Robot Arm 1 end: [-9.34532296489714;3.51440401009778;0.109062741255916]
Robot Arm 1 togoal: 20.4523358759591
Mobile Platform 2 Pose: [0.196747726192565;8.31197105948722;2.09578308921124]
Robot Arm 2 end: [7.79178712257264;1.90427807238818;0.138673818258095]
Robot Arm 1 togoal: 21.4515815527064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-0.62789756184152;-9.025316796073;3.37726613301216]
Robot Arm 1 end: [-9.34537038660262;3.5612201257003;0.108734999145512]
Robot Arm 1 togoal: 20.4376058797787
Mobile Platform 2 Pose: [0.213186155215566;8.30074602204282;1.99193571314476]
Robot Arm 2 end: [7.77961295677733;1.80340818892839;0.138606227622404]
Robot Arm 1 togoal: 21.3856515988916

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-0.626909336235362;-9.02521451757909;3.42253104431054]
Robot Arm 1 end: [-9.34512379543756;3.608241369185;0.108411323440869]
Robot Arm 1 togoal: 20.4226296011201
Mobile Platform 2 Pose: [0.229622610354509;8.28952246834544;1.88754823744202]
Robot Arm 2 end: [7.76745862682496;1.70199787791709;0.138540037341136]
Robot Arm 1 togoal: 21.3197217603576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-0.62754328771484;-9.02430251542336;3.46799774693429]
Robot Arm 1 end: [-9.3411000260179;3.65192787203475;0.106461668697172]
Robot Arm 1 togoal: 20.4053201065066
Mobile Platform 2 Pose: [0.246056521941689;8.27830078671127;1.78261440438977]
Robot Arm 2 end: [7.75532442904808;1.6000406558976;0.13847529916904]
Robot Arm 1 togoal: 21.2537964455209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-0.629940666843745;-9.02251523303009;3.51342024879646]
Robot Arm 1 end: [-9.34209421990064;3.70269021698898;0.107774895112739]
Robot Arm 1 togoal: 20.3904370848642
Mobile Platform 2 Pose: [0.262487305066885;8.26708137590677;1.67712782345994]
Robot Arm 2 end: [7.74321066817714;1.4975299056328;0.138412064340765]
Robot Arm 1 togoal: 21.1878802183245

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-0.635065740133225;-9.01936027057744;3.5593180322658]
Robot Arm 1 end: [-9.33583061430468;3.74685280611706;0.105834881670587]
Robot Arm 1 togoal: 20.3710305994956
Mobile Platform 2 Pose: [0.278914359168645;8.25586464536947;1.57108197214418]
Robot Arm 2 end: [7.73111765806721;1.39445887617994;0.138350383388618]
Robot Arm 1 togoal: 21.1219778062447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','1.154632e-14','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-0.643329667704568;-9.01464226234792;3.60519792028104]
Robot Arm 1 end: [-9.33142015477702;3.79547350520883;0.106520989028968]
Robot Arm 1 togoal: 20.3519214104605
Mobile Platform 2 Pose: [0.456184167686974;8.16422745364439;1.46447020927691]
Robot Arm 2 end: [7.63911171887737;1.28934584471848;0.138290306084766]
Robot Arm 1 togoal: 20.9880794245292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.804570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-0.654922290850238;-9.00826732636979;3.651470685409]
Robot Arm 1 end: [-9.32622323613532;3.84567341758478;0.10789014283862]
Robot Arm 1 togoal: 20.3316383925243
Mobile Platform 2 Pose: [0.476511110793738;8.15106655031093;1.35682597197049]
Robot Arm 2 end: [7.62402485302645;1.16795444599651;0.14484719442117]
Robot Arm 1 togoal: 20.9098841477482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.772768e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-0.670638943115348;-8.99983772732091;3.69825530083956]
Robot Arm 1 end: [-9.31853880676672;3.89489836179211;0.106549358146805]
Robot Arm 1 togoal: 20.3095760236308
Mobile Platform 2 Pose: [0.495755626492365;8.13845901000665;1.24762416411474]
Robot Arm 2 end: [7.64507723844277;1.06720040735511;0.115123618975064]
Robot Arm 1 togoal: 20.8760188734801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.754372e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-0.674912071318675;-8.99711250192041;3.74520276198684]
Robot Arm 1 end: [-9.32905357091719;3.92646557356692;0.116207484715856]
Robot Arm 1 togoal: 20.3094454475609
Mobile Platform 2 Pose: [0.515179781783563;8.12581932135378;1.14623811093812]
Robot Arm 2 end: [7.65200121165997;0.967483489472954;0.100636242753272]
Robot Arm 1 togoal: 20.8301770052779

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995782e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-0.678919551779141;-8.99453878024603;3.79288140464143]
Robot Arm 1 end: [-9.35515596349519;3.92928753427882;0.126408203202565]
Robot Arm 1 togoal: 20.3327444899202
Mobile Platform 2 Pose: [0.534734093689606;8.11315836613637;1.04929130553553]
Robot Arm 2 end: [7.65073966332243;0.868251471678278;0.0938571128479217]
Robot Arm 1 togoal: 20.7774935334936

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995616e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991390e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-0.683275896778865;-8.99176890146978;3.84800320795532]
Robot Arm 1 end: [-9.3829199732126;3.92943608104648;0.139544742038477]
Robot Arm 1 togoal: 20.3582354739259
Mobile Platform 2 Pose: [0.55476896777535;8.10029276986984;0.954985216950707]
Robot Arm 2 end: [7.6318455142356;0.761668461822159;0.102531805170449]
Robot Arm 1 togoal: 20.7052538342275

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-0.687677163901296;-8.98893895467618;3.91238493641651]
Robot Arm 1 end: [-9.40201513303639;3.95049910106788;0.150357152492536]
Robot Arm 1 togoal: 20.3694210561856
Mobile Platform 2 Pose: [0.575096241248205;8.08729583416636;0.859141543365358]
Robot Arm 2 end: [7.61022316870311;0.651561228867725;0.114159580204022]
Robot Arm 1 togoal: 20.6289824429277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997190e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993030e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-0.692110901480892;-8.98605978601062;3.98432166605589]
Robot Arm 1 end: [-9.41383745108488;3.98399654837387;0.159215704398634]
Robot Arm 1 togoal: 20.3701703128587
Mobile Platform 2 Pose: [0.595611777209948;8.07421790307864;0.760981868249123]
Robot Arm 2 end: [7.59051342780827;0.539999799993466;0.124446013310579]
Robot Arm 1 togoal: 20.5540293245753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997376e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004652e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-0.696682857223221;-8.98306965588707;4.06287229653287]
Robot Arm 1 end: [-9.4207606094671;4.02715239631725;0.166725298038509]
Robot Arm 1 togoal: 20.3635682314132
Mobile Platform 2 Pose: [0.6108525452571;8.06379377337292;0.660845641230838]
Robot Arm 2 end: [7.59056562334805;0.498464754374349;0.0995165584892151]
Robot Arm 1 togoal: 20.5345167053584

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997514e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.452723e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-0.701461482776263;-8.97992813047076;4.14746985931645]
Robot Arm 1 end: [-9.42399501569018;4.07816531353652;0.173238261976246]
Robot Arm 1 togoal: 20.3513186335883
Mobile Platform 2 Pose: [0.626269530516473;8.05325216443491;0.565154499019513]
Robot Arm 2 end: [7.58601034428692;0.440337242064111;0.0879270318373633]
Robot Arm 1 togoal: 20.5017584535931

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.876154e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-0.706475972687738;-8.97661525332725;4.2377952817095]
Robot Arm 1 end: [-9.42409412871806;4.13715359527337;0.178714865210864]
Robot Arm 1 togoal: 20.3339649372618
Mobile Platform 2 Pose: [0.641840540542259;8.04261302605698;0.471356692915225]
Robot Arm 2 end: [7.62231002880419;0.372527550894525;0.0523749911334048]
Robot Arm 1 togoal: 20.5010243149465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997438e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.726437e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-0.711959486601991;-8.97301133295797;4.33352384079339]
Robot Arm 1 end: [-9.42144890604623;4.19209049522938;0.184915617450683]
Robot Arm 1 togoal: 20.3152556445412
Mobile Platform 2 Pose: [0.657187497223927;8.03212561715878;0.38721561087208]
Robot Arm 2 end: [7.54106579166704;0.261555779588011;0.0969272859826801]
Robot Arm 1 togoal: 20.3719537408663

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998797e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001004e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.34228932066112;9.99051128672072;9.81851214072175]
Robot Arm 2 end: [9.76842055039834;9.82935460996154;1.4891554174064]
Robot Arm 1 togoal: 27.9999065959595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997583e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.755921590689592;-9.97988497819246;-9.6577319006169]
Robot Arm 1 end: [-9.69473139479082;-9.65242476486688;1.46645126276626]
Robot Arm 1 togoal: 27.8227104972978
Mobile Platform 2 Pose: [-2.32896588746572;9.98143129270071;9.63152345922832]
Robot Arm 2 end: [9.55527452694305;9.62759181043683;1.3965365907206]
Robot Arm 1 togoal: 27.7067972389716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975496e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.741041397102319;-9.96973107815118;-9.48856967281555]
Robot Arm 1 end: [-9.55458065538716;-9.4798216906108;1.39996957464794]
Robot Arm 1 togoal: 27.6018065725759
Mobile Platform 2 Pose: [-2.31569000205569;9.97239763886784;9.44692676222349]
Robot Arm 2 end: [9.3903107577824;9.35784763656426;1.25864518943501]
Robot Arm 1 togoal: 27.4003512103483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979377e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.726085750816405;-9.95952595608916;-9.32064150140879]
Robot Arm 1 end: [-9.42892607377525;-9.30814325800548;1.31005200249116]
Robot Arm 1 togoal: 27.3921898268382
Mobile Platform 2 Pose: [-2.30205961605762;9.96311052015602;9.27506167389509]
Robot Arm 2 end: [9.29103757132757;9.10220954718338;1.11576168751532]
Robot Arm 1 togoal: 27.1514780123068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970221e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.711071148155552;-9.94928082607102;-9.15383416545519]
Robot Arm 1 end: [-9.32182454994546;-9.13747218263378;1.19975272128423]
Robot Arm 1 togoal: 27.1970392744144
Mobile Platform 2 Pose: [-2.28806535339767;9.95357381961384;9.11573645719855]
Robot Arm 2 end: [9.24174970270148;8.84796300191843;0.968514637329893]
Robot Arm 1 togoal: 26.940599121383

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.696014147992118;-9.93900693215315;-8.9880018026876]
Robot Arm 1 end: [-9.23670284449871;-8.96787640810329;1.07283142514188]
Robot Arm 1 togoal: 27.0191029982884
Mobile Platform 2 Pose: [-2.27340393373388;9.94361801262514;8.97139852144633]
Robot Arm 2 end: [9.27351600324764;8.542641001358;0.777345032660175]
Robot Arm 1 togoal: 26.7553915527905

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.680930058676466;-9.92871465548676;-8.82297746775894]
Robot Arm 1 end: [-9.17623760051621;-8.79941220629422;0.933626381288688]
Robot Arm 1 togoal: 26.8605844236788
Mobile Platform 2 Pose: [-2.25895322459396;9.93379003882871;8.85794871105448]
Robot Arm 2 end: [9.36706148923141;8.30205927194424;0.626255087083789]
Robot Arm 1 togoal: 26.6617518700595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.665831815598383;-9.91841275221804;-8.6585868348378]
Robot Arm 1 end: [-9.14226012173675;-8.63212919847478;0.786906130707495]
Robot Arm 1 togoal: 26.7230472597311
Mobile Platform 2 Pose: [-2.22078608113234;9.91214666092824;8.7733682673003]
Robot Arm 2 end: [9.4803840552905;8.13339108071085;0.49785737707527]
Robot Arm 1 togoal: 26.6336639585031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982683e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.650729121699015;-9.90810777132205;-8.49466311568652]
Robot Arm 1 end: [-9.13568978254168;-8.4660764421974;0.6377047583955]
Robot Arm 1 togoal: 26.6073450969199
Mobile Platform 2 Pose: [-2.18463653653514;9.89158713004136;8.71371734205259]
Robot Arm 2 end: [9.61365700927167;8.04464070492213;0.368302201440778]
Robot Arm 1 togoal: 26.6764561301138

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.635627878258829;-9.89780367077227;-8.33106222595957]
Robot Arm 1 end: [-9.14979122173426;-8.30126073263887;0.493056253941508]
Robot Arm 1 togoal: 26.508627210497
Mobile Platform 2 Pose: [-2.15152429991123;9.87271808983322;8.67609264023361]
Robot Arm 2 end: [9.74414757333009;8.03854121251116;0.244220866328798]
Robot Arm 1 togoal: 26.7740638945431

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993178e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.62042175742754;-9.88742786307749;-8.16786274884478]
Robot Arm 1 end: [-9.17946487028817;-8.13806180322454;0.357255935201147]
Robot Arm 1 togoal: 26.4233884832465
Mobile Platform 2 Pose: [-2.1272908307909;9.85853731262392;8.65669162791756]
Robot Arm 2 end: [9.79851211309376;8.04485244590493;0.198969564345443]
Robot Arm 1 togoal: 26.8206179302342

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.605140705194117;-9.87700076826651;-8.00514089229303]
Robot Arm 1 end: [-9.18559970049075;-7.97434911767715;0.318955631894597]
Robot Arm 1 togoal: 26.3174727649618
Mobile Platform 2 Pose: [-2.11599138575357;9.85082294495502;8.64434954526863]
Robot Arm 2 end: [9.86152398127245;8.0586990219157;0.169071163245007]
Robot Arm 1 togoal: 26.8779548478386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.589843037460278;-9.86656233439354;-7.84333051322093]
Robot Arm 1 end: [-9.17526176801076;-7.81073011971916;0.318918031915389]
Robot Arm 1 togoal: 26.1984307226063
Mobile Platform 2 Pose: [-2.1047539381066;9.84315130184348;8.63984062526242]
Robot Arm 2 end: [9.91864007522033;8.06827204782905;0.159762405924321]
Robot Arm 1 togoal: 26.9270801430095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.574476956223845;-9.85607729661586;-7.68269096600947]
Robot Arm 1 end: [-9.16488386391496;-7.64827828656266;0.318876634429779]
Robot Arm 1 togoal: 26.0806365064842
Mobile Platform 2 Pose: [-2.08131127661182;9.82940651144762;8.64250786980034]
Robot Arm 2 end: [9.99588411143221;8.10978735948063;0.146298190898873]
Robot Arm 1 togoal: 27.012767067951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984043e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.559043394587716;-9.84554629251811;-7.52323027028225]
Robot Arm 1 end: [-9.15446666727764;-7.48700179297147;0.318831395976652]
Robot Arm 1 togoal: 25.9640912486406
Mobile Platform 2 Pose: [-2.05517994920749;9.81462918789848;8.65974546428492]
Robot Arm 2 end: [10.0001144552277;8.15626609258941;0.132627289821076]
Robot Arm 1 togoal: 27.0477754247829

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.543543302255094;-9.83496997099686;-7.36495605264821]
Robot Arm 1 end: [-9.14401086728869;-7.3269084214144;0.318782272337449]
Robot Arm 1 togoal: 25.8487956198283
Mobile Platform 2 Pose: [-2.0268663915342;9.7989057490715;8.68131753295892]
Robot Arm 2 end: [9.99376314211075;8.20309443336292;0.12172062042438]
Robot Arm 1 togoal: 27.0751028495712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.527977644896094;-9.82434899182775;-7.20787553909628]
Robot Arm 1 end: [-9.13351716184729;-7.16800555442587;0.318729221279988]
Robot Arm 1 togoal: 25.7347498282938
Mobile Platform 2 Pose: [-1.99663871566097;9.78232849425592;8.70502805471679]
Robot Arm 2 end: [9.9799180226131;8.24695588121182;0.113714274107002]
Robot Arm 1 togoal: 27.0948145635938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.512347403485289;-9.81368402521274;-7.05199554786409]
Robot Arm 1 end: [-9.12298625740851;-7.01030016733248;0.318672201689868]
Robot Arm 1 togoal: 25.6219536202526
Mobile Platform 2 Pose: [-1.96617063723036;9.76569620755929;8.72950087517512]
Robot Arm 2 end: [9.96404931526709;8.28679761507845;0.108462465384548]
Robot Arm 1 togoal: 27.110247036203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996139e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.496653573622041;-9.80297575131568;-6.89732248276789]
Robot Arm 1 end: [-9.11241886850294;-6.85379882146527;0.318611173604233]
Robot Arm 1 togoal: 25.5104062807587
Mobile Platform 2 Pose: [-1.93780978823696;9.75015908475713;8.7540928958741]
Robot Arm 2 end: [9.94832512663512;8.32203313033986;0.105373366456675]
Robot Arm 1 togoal: 27.1226210413524

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997344e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.480897164832203;-9.79222485978517;-6.74386232701583]
Robot Arm 1 end: [-9.10181571724627;-6.69850765785465;0.318546098244226]
Robot Arm 1 togoal: 25.4001066352272
Mobile Platform 2 Pose: [-1.93061376900218;9.74522762329417;8.77843036746112]
Robot Arm 2 end: [9.94336135263078;8.35399801206349;0.10349414480444]
Robot Arm 1 togoal: 27.1406747874468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999018e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.465079199852494;-9.78143204926559;-6.59162063751894]
Robot Arm 1 end: [-9.09117753283955;-6.54443239142245;0.318476938046057]
Robot Arm 1 togoal: 25.2910530516085
Mobile Platform 2 Pose: [-1.92350586773183;9.74035624469165;8.80258530639099]
Robot Arm 2 end: [9.93829567330936;8.38331479136992;0.102357517019835]
Robot Arm 1 togoal: 27.1568489256287

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000205e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.449200713898937;-9.7705980268973;-6.44060253971254]
Robot Arm 1 end: [-9.08050505106126;-6.39157830568398;0.31840365669065]
Robot Arm 1 togoal: 25.1832434432081
Mobile Platform 2 Pose: [-1.91644734580471;9.73551848595512;8.8265925852545]
Robot Arm 2 end: [9.93292601931904;8.41071576326919;0.101628426729391]
Robot Arm 1 togoal: 27.1715036439013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.316849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.433262753920773;-9.75972350780692;-6.29081272289949]
Robot Arm 1 end: [-9.06979901375214;-6.239950247972;0.318326219131776]
Robot Arm 1 togoal: 25.0766752721467
Mobile Platform 2 Pose: [-1.9094080244826;9.73069375867373;8.85049235803085]
Robot Arm 2 end: [9.92726100288959;8.43691221566812;0.101103532078485]
Robot Arm 1 togoal: 27.185135076088

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.278730e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.417266377841304;-9.74880921458877;-6.14225543612552]
Robot Arm 1 end: [-9.05906016829367;-6.08955262519274;0.318244591622641]
Robot Arm 1 togoal: 24.9713455534512
Mobile Platform 2 Pose: [-1.90236809113438;9.7258685682107;8.87431496556931]
Robot Arm 2 end: [9.92140177000977;8.46250854659541;0.100662066002119]
Robot Arm 1 togoal: 27.1982321146432
Step 27:
Mobile Platform 1 Pose: [0.401212653787188;-9.73785587677836;-5.99493448459504]
Robot Arm 1 end: [-9.04828926708141;-5.94038940012288;0.318158741740877]
Robot Arm 1 togoal: 24.8672508597659
Mobile Platform 2 Pose: [-1.89531821191928;9.72103646494681;8.89807655856036]
Robot Arm 2 end: [9.91436030593449;8.48479490987382;0.100596967535277]
Robot Arm 1 togoal: 27.2082144586761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.930157e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.385102659307702;-9.72686423031915;-5.8488532266347]
Robot Arm 1 end: [-9.03748706699411;-5.79246408825485;0.318068638411877]
Robot Arm 1 togoal: 24.7643873266729
Mobile Platform 2 Pose: [-1.8881940956676;9.71615375759575;8.92190783290545]
Robot Arm 2 end: [9.908380023254;8.51082430287276;0.100061598297347]
Robot Arm 1 togoal: 27.2215590564456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.571840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.368937480585549;-9.71583501702348;-5.70401457121015]
Robot Arm 1 end: [-9.02665432885965;-5.64577975519625;0.31797425193045]
Robot Arm 1 togoal: 24.6627506586054
Mobile Platform 2 Pose: [-1.88106633887888;9.71126851580549;8.94566774675127]
Robot Arm 2 end: [9.90236126893858;8.53737515838708;0.0994265240174952]
Robot Arm 1 togoal: 27.2352553318259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.352718211640781;-9.7047689840289;-5.56042097600019]
Robot Arm 1 end: [-9.01579181691897;-5.50033901462767;0.317875553980748]
Robot Arm 1 togoal: 24.5623361353394
Mobile Platform 2 Pose: [-1.87394525445638;9.70638774468134;8.96933685097462]
Robot Arm 2 end: [9.89620654966663;8.56449258164057;0.0986744763722623]
Robot Arm 1 togoal: 27.2492647566122

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.336445953529443;-9.69366688325088;-5.41807444603074]
Robot Arm 1 end: [-9.00490029828894;-5.35614402682166;0.317772517654445]
Robot Arm 1 togoal: 24.4631386190461
Mobile Platform 2 Pose: [-1.86684151718938;9.70151872187406;8.99289404384524]
Robot Arm 2 end: [9.88981915328476;8.59203678631333;0.0978201429265979]
Robot Arm 1 togoal: 27.2634219921047

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995173e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.32012181353854;-9.68252947083301;-5.27697653286952]
Robot Arm 1 end: [-8.99398054242525;-5.213196497724;0.317665117467128]
Robot Arm 1 togoal: 24.3651525618846
Mobile Platform 2 Pose: [-1.85976115888799;9.69666595399347;9.01632950221473]
Robot Arm 2 end: [9.88374889442258;8.62036057485201;0.096786203794022]
Robot Arm 1 togoal: 27.2783739062881

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994364e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.303746904378936;-9.67135750659591;-5.13712833438094]
Robot Arm 1 end: [-8.98303332058644;-5.07149767859683;0.317553329372896]
Robot Arm 1 togoal: 24.2683720141152
Mobile Platform 2 Pose: [-1.85272697501215;9.69184502244334;9.03983073450744]
Robot Arm 2 end: [9.87793694321769;8.64924090815299;0.095662864253476]
Robot Arm 1 togoal: 27.2939214756583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993878e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.287322343377793;-9.66015175348576;-4.99853049503885]
Robot Arm 1 end: [-8.97205940530007;-4.93104836622174;0.317437130777141]
Robot Arm 1 togoal: 24.1727906327106
Mobile Platform 2 Pose: [-1.84575347991132;9.68706576572156;9.06357195572674]
Robot Arm 2 end: [9.87227692480168;8.67845254994136;0.0945393812911017]
Robot Arm 1 togoal: 27.3098291780294

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.270849251672138;-9.64891297702367;-4.86118320679359]
Robot Arm 1 end: [-8.961059569832;-4.79184890365925;0.3173165005475]
Robot Arm 1 togoal: 24.0784016904415
Mobile Platform 2 Pose: [-1.83884577682504;9.6823315792505;9.08766909741265]
Robot Arm 2 end: [9.86665207891347;8.70779410127672;0.0934860957705924]
Robot Arm 1 togoal: 27.3258709084839

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.254328753405152;-9.63764194475692;-4.72508621048801]
Robot Arm 1 end: [-8.95003458765994;-4.65389918155965;0.317191419022983]
Robot Arm 1 togoal: 23.9851980854121
Mobile Platform 2 Pose: [-1.8320011924945;9.67764050324291;9.11216579211319]
Robot Arm 2 end: [9.86090649602706;8.73703560157243;0.0925552995698919]
Robot Arm 1 togoal: 27.341773246092

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.237761974926717;-9.62633942571313;-4.59023879781584]
Robot Arm 1 end: [-8.93898523195212;-4.51719864001875;0.317061868021266]
Robot Arm 1 togoal: 23.8931723510189
Mobile Platform 2 Pose: [-1.82520941994449;9.6729853907458;9.13702999351374]
Robot Arm 2 end: [9.85496384876943;8.7659533288907;0.0917791588876938]
Robot Arm 1 togoal: 27.3573256366684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985475e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.221150043999774;-9.61500618985849;-4.45663981381407]
Robot Arm 1 end: [-8.92791227505214;-4.38174627097048;0.316927830844163]
Robot Arm 1 togoal: 23.8023166663075
Mobile Platform 2 Pose: [-1.81851540669495;9.66838586525949;9.1621841827052]
Robot Arm 2 end: [9.83644506191448;8.77577410725499;0.0955818992239454]
Robot Arm 1 togoal: 27.3504307604659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987547e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.204494089013975;-9.60364300756088;-4.3242876598801]
Robot Arm 1 end: [-8.91681648797091;-4.24754062110698;0.316789292281287]
Robot Arm 1 togoal: 23.7126228666979
Mobile Platform 2 Pose: [-1.81128853999023;9.66343296396822;9.18130931559717]
Robot Arm 2 end: [9.83104351563799;8.79822696782552;0.0948973406002212]
Robot Arm 1 togoal: 27.3619698276304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.968342e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.187795238208097;-9.59225064905909;-4.19318029730241]
Robot Arm 1 end: [-8.90569863988658;-4.11457979531553;0.316646238611902]
Robot Arm 1 togoal: 23.6240824550523
Mobile Platform 2 Pose: [-1.80409344649397;9.65850204275292;9.20096291232189]
Robot Arm 2 end: [9.82600547713957;8.8204390245627;0.0943013014852319]
Robot Arm 1 togoal: 27.3736166304928
Step 41:
Mobile Platform 1 Pose: [0.17105461890263;-9.58082988393898;-4.06331525129281]
Robot Arm 1 end: [-8.89455949765334;-3.98286146062012;0.31649865760501]
Robot Arm 1 togoal: 23.5366866130534
Mobile Platform 2 Pose: [-1.79692605950571;9.65358900621638;9.2212663743938]
Robot Arm 2 end: [9.81878522549161;8.83933488865318;0.0941037744818603]
Robot Arm 1 togoal: 27.3813955031402
Step 42:
Mobile Platform 1 Pose: [0.15427335674391;-9.56938148061755;-3.93468961550677]
Robot Arm 1 end: [-8.88339982532;-3.85238285061453;0.316346538517676]
Robot Arm 1 togoal: 23.4504262128678
Mobile Platform 2 Pose: [-1.78968294733597;9.64862427371358;9.24162082981851]
Robot Arm 2 end: [9.81149555579488;8.85829253569159;0.0939093076804045]
Robot Arm 1 togoal: 27.3891792750916
Step 43:
Mobile Platform 1 Pose: [0.137452574961122;-9.55790620583581;-3.80730005703718]
Robot Arm 1 end: [-8.87222038365899;-3.72314077037239;0.316189872091636]
Robot Arm 1 togoal: 23.3652918290605
Mobile Platform 2 Pose: [-1.78236380854384;9.64360764054813;9.26201885198626]
Robot Arm 2 end: [9.80413630437882;8.87730479864958;0.0937179025647515]
Robot Arm 1 togoal: 27.396963069092
Step 44:
Mobile Platform 1 Pose: [0.120593393637415;-9.54640482416134;-3.68114282186597]
Robot Arm 1 end: [-8.86102192970673;-3.59513160181865;0.316028650548207]
Robot Arm 1 togoal: 23.2812737507346
Mobile Platform 2 Pose: [-1.77496835080634;9.63853890826486;9.28245289445682]
Robot Arm 2 end: [9.79670731826783;8.89636439173962;0.0935295600391956]
Robot Arm 1 togoal: 27.4047419204693

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.155399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.107796082874326;-9.53692762731733;-3.55621369542846]
Robot Arm 1 end: [-8.85630456050134;-3.43875348785901;0.31586286775982]
Robot Arm 1 togoal: 23.1864760688926
Mobile Platform 2 Pose: [-1.76749629118317;9.63341788483042;9.30291529234077]
Robot Arm 2 end: [9.78920845542093;8.91546391170442;0.0933442804281479]
Robot Arm 1 togoal: 27.4125107777584

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.414749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.0930337223446063;-9.52644978573735;-3.43699235205551]
Robot Arm 1 end: [-8.89278019429709;-3.31047391940539;0.247744700101602]
Robot Arm 1 togoal: 23.1457779840712
Mobile Platform 2 Pose: [-1.75994735637974;9.6282443848127;9.32339826380009]
Robot Arm 2 end: [9.78163958496845;8.93459583922301;0.0931620634763861]
Robot Arm 1 togoal: 27.4202645034159

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.660086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.078272948810822;-9.51596761735124;-3.31876475424547]
Robot Arm 1 end: [-8.93795044121188;-3.1883080692077;0.182485913906075]
Robot Arm 1 togoal: 23.1164239578876
Mobile Platform 2 Pose: [-1.75232128300825;9.62301822955895;9.34389391166937]
Robot Arm 2 end: [9.77400058744532;8.95375254043464;0.0929829083498618]
Robot Arm 1 togoal: 27.4279978746238

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.0636710786518998;-9.50555878296517;-3.20153870792454]
Robot Arm 1 end: [-8.97836301548581;-3.06591330603767;0.13507833128787]
Robot Arm 1 togoal: 23.0828319783396
Mobile Platform 2 Pose: [-1.74461781784652;9.61773924737229;9.36439422519834]
Robot Arm 2 end: [9.76629135502063;8.97292626858141;0.0928068136370716]
Robot Arm 1 togoal: 27.4357055841865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.938725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.0848849261788218;-9.51304736023152;-3.08655366876765]
Robot Arm 1 end: [-8.93251463527185;-2.8511500223301;0.223074743582477]
Robot Arm 1 togoal: 22.9189174544188
Mobile Platform 2 Pose: [-1.73683671809418;9.61240727368646;9.38489108191665]
Robot Arm 2 end: [9.7585117917232;8.99210916577048;0.0926337773510086]
Robot Arm 1 togoal: 27.4433822415207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996140e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.0707848320842534;-9.50289195460597;-2.99442628647028]
Robot Arm 1 end: [-8.97658168434988;-2.76939829600012;0.162615639250635]
Robot Arm 1 togoal: 22.9131707394726
Mobile Platform 2 Pose: [-1.72897775162594;9.60702215123854;9.40537624962139]
Robot Arm 2 end: [9.75066181366274;9.01129326485646;0.0924637969317016]
Robot Arm 1 togoal: 27.4510223737392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.0567606561318195;-9.49277208953157;-2.90334281313564]
Robot Arm 1 end: [-9.01940477928558;-2.68753041586281;0.115120442152747]
Robot Arm 1 togoal: 22.9060621570563
Mobile Platform 2 Pose: [-1.72104069724157;9.60158373023941;9.42584138848776]
Robot Arm 2 end: [9.74274134924639;9.03047049144437;0.0922968692493532]
Robot Arm 1 togoal: 27.4586204268313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917060e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.0765692845258019;-9.49956579604923;-2.81427298431748]
Robot Arm 1 end: [-8.97700972579405;-2.50224886599872;0.204863594563356]
Robot Arm 1 togoal: 22.7632453722654
Mobile Platform 2 Pose: [-1.71302534491234;9.59609186854177;9.4462780533031]
Robot Arm 2 end: [9.73475033939015;9.0496326660135;0.0921329906080856]
Robot Arm 1 togoal: 27.4661707669387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994405e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.0639933884914072;-9.49025032011621;-2.74330794247825]
Robot Arm 1 end: [-8.99842125933006;-2.43500939059642;0.174143437041907]
Robot Arm 1 togoal: 22.7460654068428
Mobile Platform 2 Pose: [-1.70492015605758;9.59054076182251;9.46667763867709]
Robot Arm 2 end: [9.73077120164443;9.08486792195379;0.0899402513279543]
Robot Arm 1 togoal: 27.4878781977498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997316e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992911e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.0510805824512108;-9.48077491161159;-2.67453352203139]
Robot Arm 1 end: [-9.03787174242689;-2.38574673474698;0.124739994275825]
Robot Arm 1 togoal: 22.7551494097924
Mobile Platform 2 Pose: [-1.69707767435588;9.58517086497482;9.48846535504039]
Robot Arm 2 end: [9.72869304517127;9.12282544258568;0.0878848682309121]
Robot Arm 1 togoal: 27.5128624675641

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.921506e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.0637190389532063;-9.48407797641509;-2.60719412868282]
Robot Arm 1 end: [-8.98324666460307;-2.2357895077056;0.222765696563244]
Robot Arm 1 togoal: 22.6221928891224
Mobile Platform 2 Pose: [-1.68957313024101;9.58002835234064;9.51245814534652]
Robot Arm 2 end: [9.72125195925949;9.14549238051308;0.0876845930617512]
Robot Arm 1 togoal: 27.5233001362236

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996932e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.0527933052463872;-9.47568864136357;-2.54943829910505]
Robot Arm 1 end: [-8.99051004449548;-2.17509247432488;0.208478836560233]
Robot Arm 1 togoal: 22.5963505972297
Mobile Platform 2 Pose: [-1.68199224992798;9.5748337440456;9.53644923825187]
Robot Arm 2 end: [9.71374203365026;9.16816963490796;0.0874876897257572]
Robot Arm 1 togoal: 27.5337122391011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997325e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.0417834640606516;-9.46727711639077;-2.49366451549405]
Robot Arm 1 end: [-9.00581978318222;-2.12596471905958;0.181763810523043]
Robot Arm 1 togoal: 22.5843698067908
Mobile Platform 2 Pose: [-1.67433480483944;9.56958688518137;9.56043086986914]
Robot Arm 2 end: [9.70616317783111;9.19084968867292;0.0872941595856699]
Robot Arm 1 togoal: 27.5440935846759

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.879770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.0256881165589426;-9.45633666139766;-2.43931150048308]
Robot Arm 1 end: [-8.98322897504817;-2.15798564813381;0.139747183748191]
Robot Arm 1 togoal: 22.5851158217473
Mobile Platform 2 Pose: [-1.66660057672071;9.56428762789913;9.5843951781523]
Robot Arm 2 end: [9.69851531272083;9.21352492681997;0.0871040035245774]
Robot Arm 1 togoal: 27.5544389049702

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.719529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.00811235008871498;-9.44453943480665;-2.36928902396346]
Robot Arm 1 end: [-8.87496815249426;-2.16402076684318;0.211597894033202]
Robot Arm 1 togoal: 22.4931624688254
Mobile Platform 2 Pose: [-1.65878935786232;9.55893583156131;9.60833420497513]
Robot Arm 2 end: [9.69079837086003;9.23618763845099;0.0869172219491424]
Robot Arm 1 togoal: 27.5647428567339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-0.00925963423576216;-9.43269961657713;-2.27063476756489]
Robot Arm 1 end: [-8.91365212315205;-2.10939938509289;0.146267371792176]
Robot Arm 1 togoal: 22.5000817754594
Mobile Platform 2 Pose: [-1.65090095131878;9.5535313628907;9.63223989831637]
Robot Arm 2 end: [9.68301229659689;9.25883001884257;0.0867338147932841]
Robot Arm 1 togoal: 27.5750000227141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005107e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-0.0270477069123037;-9.42064146827064;-2.16358996071899]
Robot Arm 1 end: [-8.83760476059729;-2.08308569513343;0.209354995625488]
Robot Arm 1 togoal: 22.4182230707502
Mobile Platform 2 Pose: [-1.64293517112328;9.54807409611681;9.65610411455091]
Robot Arm 2 end: [9.67515704626767;9.2814441716351;0.0865537815223212]
Robot Arm 1 togoal: 27.5852049130124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.037674e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-0.0442627981181342;-9.40889181698849;-2.04280613294302]
Robot Arm 1 end: [-8.78005572378334;-1.92796231790181;0.288012121409071]
Robot Arm 1 togoal: 22.2819890753596
Mobile Platform 2 Pose: [-1.63489184249815;9.54256391311927;9.67991862084659]
Robot Arm 2 end: [9.66723258837191;9.30402211112544;0.0863771211375781]
Robot Arm 1 togoal: 27.5953519665268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.215522e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-0.0615831843695151;-9.39706745635597;-1.9359135800193]
Robot Arm 1 end: [-8.7278377584046;-1.78567685381045;0.359112572584883]
Robot Arm 1 togoal: 22.1581603117577
Mobile Platform 2 Pose: [-1.6267708020606;9.53700070356805;9.70367509766577]
Robot Arm 2 end: [9.65923890374181;9.32655576466257;0.0862038321814546]
Robot Arm 1 togoal: 27.6054355524812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-0.0789633531210082;-9.38521337299716;-1.83842374500752]
Robot Arm 1 end: [-8.76854202598746;-1.69436839369723;0.266449748216261]
Robot Arm 1 togoal: 22.1492735375906
Mobile Platform 2 Pose: [-1.61857189802363;9.53138436506036;9.72736514137086]
Robot Arm 2 end: [9.65117598570568;9.34903697514535;0.0860339127429632]
Robot Arm 1 togoal: 27.6154499720409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-0.095829217512213;-9.37360239984574;-1.73706766538105]
Robot Arm 1 end: [-8.75824617575353;-1.47051159646529;0.341263541583758]
Robot Arm 1 togoal: 22.0189966616586
Mobile Platform 2 Pose: [-1.61029499039172;9.52571480325392;9.75098026693248]
Robot Arm 2 end: [9.64304384024504;9.37145750362139;0.0858673604637325]
Robot Arm 1 togoal: 27.6253894600141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-0.112791936464913;-9.36203639754261;-1.6644165138744]
Robot Arm 1 end: [-8.78689664273122;-1.39582261332366;0.27477015687564]
Robot Arm 1 togoal: 22.0083017818481
Mobile Platform 2 Pose: [-1.60193995115099;9.51999193199656;9.77451191073924]
Robot Arm 2 end: [9.63484248614528;9.39380903198569;0.0857041725444761]
Robot Arm 1 togoal: 27.6352481866398

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-0.129396866269801;-9.35064976132063;-1.5932468652786]
Robot Arm 1 end: [-8.79171411496255;-1.21452492878126;0.346309825455268]
Robot Arm 1 togoal: 21.9151542491243
Mobile Platform 2 Pose: [-1.59350666445373;9.51421567345184;9.79795143350735]
Robot Arm 2 end: [9.62657195513942;9.41608316577775;0.0855443457519278]
Robot Arm 1 togoal: 27.6450202594604

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000314e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-0.145158800022522;-9.3399016365829;-1.54402866910565]
Robot Arm 1 end: [-8.82206311913605;-1.13649629945319;0.30724996697159]
Robot Arm 1 togoal: 21.9035339393248
Mobile Platform 2 Pose: [-1.58499502679673;9.50838595822051;9.82129012330744]
Robot Arm 2 end: [9.61823229196277;9.43827143703896;0.0853878764232351]
Robot Arm 1 togoal: 27.6546997251964

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-0.160508472173415;-9.32943904724991;-1.50140309230204]
Robot Arm 1 end: [-8.82477686272387;-1.05416786812281;0.317641510793604]
Robot Arm 1 togoal: 21.863552383854
Mobile Platform 2 Pose: [-1.57640494719069;9.50250272545586;9.84451919860694]
Robot Arm 2 end: [9.60982355472573;9.46036530740679;0.0852347604831589]
Robot Arm 1 togoal: 27.6642805720337

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999036e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-0.175268759402509;-9.3193734538381;-1.46459819838085]
Robot Arm 1 end: [-8.87428729156792;-0.981597862616839;0.285455148383691]
Robot Arm 1 togoal: 21.8714604784689
Mobile Platform 2 Pose: [-1.56773634733118;9.49656592298005;9.86762981153553]
Robot Arm 2 end: [9.6013458147948;9.48235617109547;0.0850849934427035]
Robot Arm 1 togoal: 27.6737567315074

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003953e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-0.185522778207089;-9.31173048585551;-1.4361047868167]
Robot Arm 1 end: [-8.85842266692336;-0.951193732445333;0.2872211686282]
Robot Arm 1 togoal: 21.842411951399
Mobile Platform 2 Pose: [-1.5589891617545;9.49057550739024;9.89061305105806]
Robot Arm 2 end: [9.59279915739894;9.50423535827749;0.0849385704233575]
Robot Arm 1 togoal: 27.6831220812027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003952e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-0.202434155657624;-9.30077490473552;-1.40765529855397]
Robot Arm 1 end: [-8.85437426903231;-0.928362762271858;0.261537783651025]
Robot Arm 1 togoal: 21.82894378606
Mobile Platform 2 Pose: [-1.55016514592819;9.48453234813361;9.91345991475261]
Robot Arm 2 end: [9.58675977423132;9.54181145347026;0.084025118308774]
Robot Arm 1 togoal: 27.7053866630126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004086e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000127e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-0.224969652333783;-9.28705869164597;-1.38044703687034]
Robot Arm 1 end: [-8.85535571757196;-0.909397845355142;0.229675402342992]
Robot Arm 1 togoal: 21.8221657907946
Mobile Platform 2 Pose: [-1.53849403987109;9.47711142802956;9.93645073532353]
Robot Arm 2 end: [9.55171386569035;9.56307078337663;0.0813841773291216]
Robot Arm 1 togoal: 27.6957712887386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004183e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000217e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-0.25176831420852;-9.27129054726459;-1.35518542642949]
Robot Arm 1 end: [-8.86050844716417;-0.892900625964627;0.198199142223749]
Robot Arm 1 togoal: 21.8202627170716
Mobile Platform 2 Pose: [-1.52550300967503;9.46896722181799;9.95746212510399]
Robot Arm 2 end: [9.51126342729968;9.57508924123196;0.0797416844123621]
Robot Arm 1 togoal: 27.6758151616881

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004155e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000266e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-0.281397703982646;-9.25421216025062;-1.33236843961646]
Robot Arm 1 end: [-8.87201120527053;-0.877149329161735;0.169074863796774]
Robot Arm 1 togoal: 21.8241354017665
Mobile Platform 2 Pose: [-1.51171816043416;9.46032722874439;9.9751901036124]
Robot Arm 2 end: [9.47020011933422;9.58164791180184;0.0792971349041688]
Robot Arm 1 togoal: 27.6515485060045

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003957e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000050e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-0.312527114981464;-9.23651546844717;-1.31243263130003]
Robot Arm 1 end: [-8.89545780437456;-0.85923694770781;0.142841962512767]
Robot Arm 1 togoal: 21.837139746305
Mobile Platform 2 Pose: [-1.49757905422338;9.45140171721259;9.98891793109147]
Robot Arm 2 end: [9.4384698175602;9.64304902069137;0.0471083013585008]
Robot Arm 1 togoal: 27.6744618812509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004081e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000162e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-0.339920459638673;-9.22085615150167;-1.29584132695488]
Robot Arm 1 end: [-8.93774853760062;-0.828136441808661;0.128825856362863]
Robot Arm 1 togoal: 21.8592087533385
Mobile Platform 2 Pose: [-1.48293570154628;9.44221675420928;10.0003475441903]
Robot Arm 2 end: [9.40632859781016;9.6784096015763;0.0302635517803315]
Robot Arm 1 togoal: 27.67792783522

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003837e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000371e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-0.354617765703213;-9.21176985549974;-1.28262816522885]
Robot Arm 1 end: [-8.97360471612744;-0.819049974940496;0.118293863296889]
Robot Arm 1 togoal: 21.8864593479635
Mobile Platform 2 Pose: [-1.46774332178433;9.43270870041547;10.0084669745541]
Robot Arm 2 end: [9.37145091788583;9.68086632591551;0.028009986710232]
Robot Arm 1 togoal: 27.6553544396451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002277e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000408e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-0.367935501043627;-9.20355232922041;-1.27068995467316]
Robot Arm 1 end: [-9.02431671172908;-0.834909248767336;0.0957202274025297]
Robot Arm 1 togoal: 21.9397198525271
Mobile Platform 2 Pose: [-1.46294936824195;9.42828700198111;10.0120588222467]
Robot Arm 2 end: [9.26783098635022;9.62493596479186;0.0608368996475694]
Robot Arm 1 togoal: 27.5412327033553

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001657e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-0.379966548813616;-9.19617370188519;-1.25940180990799]
Robot Arm 1 end: [-9.07744704673747;-0.851311114095773;0.0822548380086305]
Robot Arm 1 togoal: 21.9947704320169
Mobile Platform 2 Pose: [-1.45542638022582;9.42226325188388;9.99665448442751]
Robot Arm 2 end: [9.18730919399156;9.61715298631408;0.0882462167143783]
Robot Arm 1 togoal: 27.477960419176

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001050e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-0.390703563635646;-9.18963092830276;-1.24780662489891]
Robot Arm 1 end: [-9.13081750726557;-0.867791273510838;0.07603477296763]
Robot Arm 1 togoal: 22.0496029480857
Mobile Platform 2 Pose: [-1.44377325801166;9.41410102017077;9.95477225544096]
Robot Arm 2 end: [9.13471484736331;9.62090777570072;0.0856535443625517]
Robot Arm 1 togoal: 27.4440804852371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000356e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-0.40057419058829;-9.18369544005598;-1.23488449579966]
Robot Arm 1 end: [-9.189651682655;-0.884457906623334;0.0756298994776542]
Robot Arm 1 togoal: 22.108895276566
Mobile Platform 2 Pose: [-1.43149761743984;9.40569123919734;9.89421371830796]
Robot Arm 2 end: [9.11345679219295;9.59023682458623;0.0805613888174863]
Robot Arm 1 togoal: 27.4075988146168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-0.409034290951932;-9.17863495590562;-1.21927073419068]
Robot Arm 1 end: [-9.24176947411669;-0.899883494904215;0.079457638224838]
Robot Arm 1 togoal: 22.1614800290578
Mobile Platform 2 Pose: [-1.41967929453873;9.39757865431903;9.82580519208111]
Robot Arm 2 end: [9.13181663590407;9.56679171036576;0.0583209658686099]
Robot Arm 1 togoal: 27.4048385432861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992145e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-0.417130174645385;-9.17389185288378;-1.19997047252369]
Robot Arm 1 end: [-9.29859975808819;-0.915626877254222;0.0872871603992423]
Robot Arm 1 togoal: 22.2180646153699
Mobile Platform 2 Pose: [-1.40852759751176;9.38993937828072;9.7516843812646]
Robot Arm 2 end: [9.13724185407098;9.52767657414122;0.0531555772387938]
Robot Arm 1 togoal: 27.3809924110669

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-0.424212341046743;-9.16978165613255;-1.17515672775774]
Robot Arm 1 end: [-9.34905561708456;-0.927549453616667;0.0971931679352921]
Robot Arm 1 togoal: 22.267117375837
Mobile Platform 2 Pose: [-1.39786770207992;9.3826634865511;9.67803080343481]
Robot Arm 2 end: [9.16378838855065;9.46561115231591;0.0508928692775215]
Robot Arm 1 togoal: 27.3554750993309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-0.430391775623832;-9.16621632192665;-1.14382842944668]
Robot Arm 1 end: [-9.39163469414812;-0.933207364982963;0.10794128397568]
Robot Arm 1 togoal: 22.306215598616
Mobile Platform 2 Pose: [-1.3876541745639;9.37570089914131;9.62412879926499]
Robot Arm 2 end: [9.18237740879465;9.42506115877991;0.0535146474377891]
Robot Arm 1 togoal: 27.3395339633974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997006e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991037e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-0.435910159459682;-9.16304492639892;-1.10551854054009]
Robot Arm 1 end: [-9.42711225989769;-0.931729868388132;0.118974232319657]
Robot Arm 1 togoal: 22.3356460602176
Mobile Platform 2 Pose: [-1.3779105331978;9.36906612052551;9.58921496029319]
Robot Arm 2 end: [9.19143991370603;9.39885602883851;0.0621997319674591]
Robot Arm 1 togoal: 27.3268197822397

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-0.436435439003887;-9.16240956081613;-1.06006328631204]
Robot Arm 1 end: [-9.45419511206975;-0.937043445550557;0.118595576328677]
Robot Arm 1 togoal: 22.3618283008316
Mobile Platform 2 Pose: [-1.36865207393624;9.36275434385783;9.57038075707312]
Robot Arm 2 end: [9.19577085978476;9.38808041891522;0.0681902264595154]
Robot Arm 1 togoal: 27.3218948367382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958863e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-0.43888862066621;-9.16074870087414;-1.00585818370062]
Robot Arm 1 end: [-9.4527314257299;-0.881742447514458;0.118256217458216]
Robot Arm 1 togoal: 22.333579398186
Mobile Platform 2 Pose: [-1.3598188300123;9.3567133387524;9.56120210454301]
Robot Arm 2 end: [9.15443385670884;9.31688490434895;0.0752382609071364]
Robot Arm 1 togoal: 27.2419640588766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-0.441358645613706;-9.1590763679309;-0.951262275944749]
Robot Arm 1 end: [-9.45125646098917;-0.826048036035901;0.117918945800017]
Robot Arm 1 togoal: 22.3052310105586
Mobile Platform 2 Pose: [-1.34570427211716;9.34780936265749;9.55318758384191]
Robot Arm 2 end: [9.1227402113665;9.28463836208273;0.0738490626925729]
Robot Arm 1 togoal: 27.1968879797501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983835e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-0.443845436506384;-9.15739261494702;-0.896269809026899]
Robot Arm 1 end: [-9.44977025301369;-0.769954445217743;0.117583777491907]
Robot Arm 1 togoal: 22.2767829531259
Mobile Platform 2 Pose: [-1.3309878987609;9.3384489546924;9.54162948592576]
Robot Arm 2 end: [9.09260718102657;9.2512002285411;0.0754311081894796]
Robot Arm 1 togoal: 27.1519065376511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-0.446348914012965;-9.15569749624396;-0.840874950948742]
Robot Arm 1 end: [-9.4482728379275;-0.713455831043735;0.117250728662926]
Robot Arm 1 togoal: 22.248235081439
Mobile Platform 2 Pose: [-1.31576448350889;9.32869483371704;9.52609574612596]
Robot Arm 2 end: [9.0641651189074;9.21613007884457;0.0789677188130835]
Robot Arm 1 togoal: 27.1068534505211

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-0.448868996747655;-9.15399106754721;-0.785071790464303]
Robot Arm 1 end: [-9.44676425284623;-0.656546270108006;0.116919815432171]
Robot Arm 1 togoal: 22.2195872931391
Mobile Platform 2 Pose: [-1.29070604389062;9.31390106738403;9.50644525497681]
Robot Arm 2 end: [9.03258783170034;9.17930506743223;0.0838352144013918]
Robot Arm 1 togoal: 27.0582812658525

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973457e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-0.451405601205036;-9.1522733860307;-0.728854335796306]
Robot Arm 1 end: [-9.44524453591164;-0.5992197583277;0.116591053907626]
Robot Arm 1 togoal: 22.1908395297382
Mobile Platform 2 Pose: [-1.26565019260296;9.2990030793086;9.48261962231673]
Robot Arm 2 end: [9.00258006241568;9.14059658041745;0.0895347153386025]
Robot Arm 1 togoal: 27.0094350060122

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965460e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-0.453958641693013;-9.15054451036257;-0.672216513335793]
Robot Arm 1 end: [-9.44371372632735;-0.541470209638879;0.116264460184967]
Robot Arm 1 togoal: 22.1619917784682
Mobile Platform 2 Pose: [-1.24072054122612;9.28407745835435;9.45467946845228]
Robot Arm 2 end: [8.9741173533172;9.09986047026315;0.0956729907211262]
Robot Arm 1 togoal: 26.9602180333237

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957663e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-0.45652803026377;-9.14880450075231;-0.615152166325156]
Robot Arm 1 end: [-9.44217186439559;-0.483291454675816;0.115940050346363]
Robot Arm 1 togoal: 22.1330440742003
Mobile Platform 2 Pose: [-1.21602308810672;9.26919027726473;9.42277414627375]
Robot Arm 2 end: [8.94712350116293;9.05704756345074;0.101951364578016]
Robot Arm 1 togoal: 26.910560191747

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963285e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-0.459113676642659;-9.14705341899926;-0.557655053524726]
Robot Arm 1 end: [-9.44061899155512;-0.424677239433848;0.115617840459238]
Robot Arm 1 togoal: 22.103996501437
Mobile Platform 2 Pose: [-1.19164782771127;9.25439793659515;9.38710648309944]
Robot Arm 2 end: [8.92149722472439;9.01217536711746;0.108149256877982]
Robot Arm 1 togoal: 26.860417283386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981590e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-0.461715488154968;-9.14529132854262;-0.499718847863127]
Robot Arm 1 end: [-9.43905515042031;-0.365621223915982;0.115297846575026]
Robot Arm 1 togoal: 22.0748491963801
Mobile Platform 2 Pose: [-1.16767213132866;9.23974898639996;9.34790876558176]
Robot Arm 2 end: [8.89714384658574;8.96529934391977;0.114106630309947]
Robot Arm 1 togoal: 26.8097738468029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978954e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-0.4643333696505;-9.14351829451289;-0.441337135071626]
Robot Arm 1 end: [-9.4374803848214;-0.30611698076349;0.114980084727901]
Robot Arm 1 togoal: 22.0456023490772
Mobile Platform 2 Pose: [-1.14416217767113;9.22528501177576;9.30542971038783]
Robot Arm 2 end: [8.87399615565436;8.91655657841642;0.11976866315326]
Robot Arm 1 togoal: 26.7586863079533

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974848e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-0.466967223425885;-9.14173438378491;-0.382503412302735]
Robot Arm 1 end: [-9.43589473984606;-0.24615799387077;0.114664570933475]
Robot Arm 1 togoal: 22.0162562056492
Mobile Platform 2 Pose: [-1.12117668256116;9.21104209351349;9.25991152588121]
Robot Arm 2 end: [8.85209070210956;8.86628320038135;0.125312879382302]
Robot Arm 1 togoal: 26.7074129817213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967332e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-0.469616949144576;-9.13993966503243;-0.323211086733397]
Robot Arm 1 end: [-9.43429826188217;-0.185737656984778;0.114351321187484]
Robot Arm 1 togoal: 21.9868110706014
Mobile Platform 2 Pose: [-1.0987440700997;9.19703773915038;9.21154557605399]
Robot Arm 2 end: [8.83130409367082;8.81456195175998;0.130617512306302]
Robot Arm 1 togoal: 26.6559359011647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.007467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-0.47228244375444;-9.13813420878438;-0.263453474153095]
Robot Arm 1 end: [-9.43269099866194;-0.124849272289414;0.114040351464433]
Robot Arm 1 togoal: 21.9572673092228
Mobile Platform 2 Pose: [-1.07689579606172;9.18329106014953;9.16053863924521]
Robot Arm 2 end: [8.81156102709868;8.76148913628061;0.135594830806196]
Robot Arm 1 togoal: 26.6042787981532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-0.474963601402872;-9.13631808748276;-0.203223797537298]
Robot Arm 1 end: [-9.43107299930744;-0.0634860489752401;0.113731677716227]
Robot Arm 1 togoal: 21.9276253500747
Mobile Platform 2 Pose: [-0.748646480990441;9.01631277317718;9.10711316282566]
Robot Arm 2 end: [8.64347590459613;8.70637724928366;0.133762195027068]
Robot Arm 1 togoal: 26.4467799991855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-0.477660313349373;-9.1344913755423;-0.142515185606694]
Robot Arm 1 end: [-9.42944431437748;-0.00164110179502078;0.11342531587076]
Robot Arm 1 togoal: 21.897885687573
Mobile Platform 2 Pose: [-0.733179741989982;9.0057406573068;9.05270822238668]
Robot Arm 2 end: [8.63032869549385;8.65422007045971;0.133839277666597]
Robot Arm 1 togoal: 26.4006304585491

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-0.480372467875487;-9.13265414941194;-0.0813206713727368]
Robot Arm 1 end: [-9.42780499591598;0.0606925503944165;0.113121281830478]
Robot Arm 1 togoal: 21.8680488846673
Mobile Platform 2 Pose: [-0.717692152016878;8.99515444291953;8.99776181009402]
Robot Arm 2 end: [8.61718610227715;8.60153694379548;0.133915225003069]
Robot Arm 1 togoal: 26.3541419744836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-0.483099950192045;-9.13080648763808;-0.0196331906700438]
Robot Arm 1 end: [-9.42615509750183;0.123521986103416;0.112819591470912]
Robot Arm 1 togoal: 21.8381155756202
Mobile Platform 2 Pose: [-0.702184078179625;8.9845543814129;8.94227461541115]
Robot Arm 2 end: [8.60404846708304;8.54832832768052;0.133990057224537]
Robot Arm 1 togoal: 26.3073164670661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-0.48584264234361;-9.12894847092983;0.0425544193236852]
Robot Arm 1 end: [-9.4244946743003;0.186854382693194;0.112520260639164]
Robot Arm 1 togoal: 21.8080864688899
Mobile Platform 2 Pose: [-0.686655885187966;8.97394072263772;8.88624733731221]
Robot Arm 2 end: [8.59091612601672;8.49459468935098;0.13406379575288]
Robot Arm 1 togoal: 26.2601558861064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-0.488600423110064;-9.12708018222608;0.105249421579557]
Robot Arm 1 end: [-9.42282378311601;0.250697017871287;0.112223305152369]
Robot Arm 1 togoal: 21.7779623501205
Mobile Platform 2 Pose: [-0.671107935176878;8.96331371463179;8.82968068012642]
Robot Arm 2 end: [8.57778940971493;8.44033650048891;0.134136463004589]
Robot Arm 1 togoal: 26.212662208811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-0.491373167905225;-9.12520170676477;0.168459180720398]
Robot Arm 1 end: [-9.42114248244762;0.315057271197988;0.11192874079611]
Robot Arm 1 togoal: 21.747744085243
Mobile Platform 2 Pose: [-0.655540588549269;8.95267360417193;8.77257534955304]
Robot Arm 2 end: [8.56466864086324;8.38555423454018;0.134208083214232]
Robot Arm 1 togoal: 26.1648374364667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-0.494160748672421;-9.12331313215405;0.232191164533852]
Robot Arm 1 end: [-9.41945083254415;0.3799426256029;0.111636583322789]
Robot Arm 1 togoal: 21.717432623691
Mobile Platform 2 Pose: [-0.639954202765028;8.9420206359726;8.71493204777738]
Robot Arm 2 end: [8.55155413886762;8.3302483597204;0.134278680915782]
Robot Arm 1 togoal: 26.116683593312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-0.496963033776935;-9.1214145484457;0.296452945494728]
Robot Arm 1 end: [-9.41774889546306;0.44536066891068;0.11134684844996]
Robot Arm 1 togoal: 21.6870290017358
Mobile Platform 2 Pose: [-0.624349133464046;8.93135505351563;8.65675147004626]
Robot Arm 2 end: [8.53844621549401;8.27441933767101;0.134348282401234]
Robot Arm 1 togoal: 26.0682027228524

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-0.499779887895208;-9.11950604821072;0.361252202295593]
Robot Arm 1 end: [-9.41603673513016;0.51131909537496;0.111059551858611]
Robot Arm 1 togoal: 21.6565343459434
Mobile Platform 2 Pose: [-0.608725734562475;8.92067709910618;8.59803429978962]
Robot Arm 2 end: [8.525345176695;8.21806761833472;0.13441691506544]
Robot Arm 1 togoal: 26.0193968860414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-0.502611171900731;-9.11758772661718;0.426596721384503]
Robot Arm 1 end: [-9.41431441740136;0.57782570721931;0.110774709191397]
Robot Arm 1 togoal: 21.625949876759
Mobile Platform 2 Pose: [-0.593084357202315;8.90998701304863;8.53878120494761]
Robot Arm 2 end: [8.5122513244487;8.16119363459362;0.134484606924582]
Robot Arm 1 togoal: 25.9702681593393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-0.50545674274651;-9.11565968151036;0.492494398508664]
Robot Arm 1 end: [-9.4125820101263;0.644888416184063;0.110492336050823]
Robot Arm 1 togoal: 21.5952769122229
Mobile Platform 2 Pose: [-0.577425351548133;8.89928503498507;8.47899283381379]
Robot Arm 2 end: [8.49916495462363;8.10379780036427;0.13455138725982]
Robot Arm 1 togoal: 25.9208186323391

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-0.508316453344006;-9.11372201349541;0.558953240262722]
Robot Arm 1 end: [-9.41083958321405;0.712515245077652;0.110212447997367]
Robot Arm 1 togoal: 21.5645168718214
Mobile Platform 2 Pose: [-0.561749065160703;8.88857140282987;8.41866981117596]
Robot Arm 2 end: [8.4860863579889;8.04588050522169;0.134617286331905]
Robot Arm 1 togoal: 25.8710504052797

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-0.511190152438468;-9.11177482602232;0.625981365640273]
Robot Arm 1 end: [-9.4090872087007;0.780714329331057;0.109935060547548]
Robot Arm 1 togoal: 21.5336712804785
Mobile Platform 2 Pose: [-0.546055844643291;8.87784635372352;8.35781273431349]
Robot Arm 2 end: [8.47301582138072;7.98744211065592;0.134682335177715]
Robot Arm 1 togoal: 25.8209655878509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-0.514077684480528;-9.10981822547356;0.693587007587047]
Robot Arm 1 end: [-9.40732496081916;0.849493918553816;0.109660189171937]
Robot Arm 1 togoal: 21.5027417726914
Mobile Platform 2 Pose: [-0.530346034802414;8.86711012357833;8.29642216920311]
Robot Arm 2 end: [8.45995362357543;7.92848294901332;0.134746566198063]
Robot Arm 1 togoal: 25.770566296074

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-0.516978889493976;-9.10785232125428;0.761778514554116]
Robot Arm 1 end: [-9.40555291607108;0.918862378089922;0.10938784929309]
Robot Arm 1 togoal: 21.4717300968163
Mobile Platform 2 Pose: [-0.514619978802554;8.85636294713368;8.23449864614212]
Robot Arm 2 end: [8.44690004033809;7.86900331480897;0.134810012674284]
Robot Arm 1 togoal: 25.7198546505193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-0.519893602939591;-9.10587722588513;0.830564352049346]
Robot Arm 1 end: [-9.40377115330093;0.988828190571814;0.109118056283429]
Robot Arm 1 togoal: 21.440638119508
Mobile Platform 2 Pose: [-0.498878018676575;8.84560505834777;8.1720426565039]
Robot Arm 2 end: [8.43385534344898;7.80900346390191;0.134872708361008]
Robot Arm 1 togoal: 25.6688327757438

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-0.522821655574937;-9.10389305509798;0.899953104185153]
Robot Arm 1 end: [-9.40197975377245;1.05939995747052;0.108850825463035]
Robot Arm 1 togoal: 21.409467830318
Mobile Platform 2 Pose: [-0.48312049560896;8.83483669055456;8.10905464880279]
Robot Arm 2 end: [8.42081979965893;7.74848360961849;0.134934688067352]
Robot Arm 1 togoal: 25.6175027975278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-0.525762873309992;-9.10189992793433;0.969953475220497]
Robot Arm 1 end: [-9.40017880124739;1.13058640063987;0.10858617209738]
Robot Arm 1 togoal: 21.3782213464576
Mobile Platform 2 Pose: [-0.467347749170729;8.8240580760551;8.04553502515439]
Robot Arm 2 end: [8.40779367139424;7.687443918582;0.134995987390376]
Robot Arm 1 togoal: 25.565866841251

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-0.528717077058516;-9.09989796684672;1.04057429109488]
Robot Arm 1 end: [-9.39836838206667;1.2023963638525;0.108324111394964]
Robot Arm 1 togoal: 21.3469009177296
Mobile Platform 2 Pose: [-0.451560118416517;8.81326944665163;7.98148413758819]
Robot Arm 2 end: [8.39477721689888;7.62588450753184;0.135056642709547]
Robot Arm 1 togoal: 25.513927030581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-0.531684082585032;-9.09788729780309;1.11182450095197]
Robot Arm 1 end: [-9.39654858523403;1.27483881432535;0.10806465850488]
Robot Arm 1 togoal: 21.3155089316346
Mobile Platform 2 Pose: [-0.435757941634488;8.80247103364931;7.91690228455729]
Robot Arm 2 end: [8.38177069117397;7.56380543933505;0.135116690934138]
Robot Arm 1 togoal: 25.4616854862373

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-0.534663700347314;-9.09586805039419;1.18371317865021]
Robot Arm 1 end: [-9.39471950250222;1.34792284423185;0.10780782851428]
Robot Arm 1 togoal: 21.2840479186562
Mobile Platform 2 Pose: [-0.419941556361965;8.79166306779637;7.85178970753965]
Robot Arm 2 end: [8.36877434382438;7.50120672069658;0.135176170100154]
Robot Arm 1 togoal: 25.409144323696

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-0.537655735334253;-9.09384035794422;1.25624952425783]
Robot Arm 1 end: [-9.39288122846186;1.42165767219829;0.107553636445756]
Robot Arm 1 togoal: 21.2525205577321
Mobile Platform 2 Pose: [-0.404111299576702;8.78084577942212;7.78614658731261]
Robot Arm 2 end: [8.35578842086867;7.43808829758747;0.135235118899979]
Robot Arm 1 togoal: 25.3563056522799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-0.540659986898999;-9.09180435762453;1.32944286552904]
Robot Arm 1 end: [-9.39103386063288;1.49605264478121;0.107302097254618]
Robot Arm 1 togoal: 21.2209296819148
Mobile Platform 2 Pose: [-0.388267508526008;8.77001939903297;7.71997304072065]
Robot Arm 2 end: [8.34281316753367;7.37445005082141;0.135293576059558]
Robot Arm 1 togoal: 25.3031715751383

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-0.543676248587247;-9.08976019057081;1.40330265935854]
Robot Arm 1 end: [-9.38917749955887;1.57111723792267;0.107053225826062]
Robot Arm 1 togoal: 21.1892782842276
Mobile Platform 2 Pose: [-0.37241051953366;8.75918415653313;7.65326911755122]
Robot Arm 2 end: [8.32984882187383;7.31029179694772;0.135351581525965]
Robot Arm 1 togoal: 25.2497441861862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-0.546704307960551;-9.08770800200355;1.47783849321075]
Robot Arm 1 end: [-9.3873122489041;1.64686105838016;0.106807036972245]
Robot Arm 1 togoal: 21.1575695237234
Mobile Platform 2 Pose: [-0.356540669495107;8.74834028211178;7.58603479692382]
Robot Arm 2 end: [8.31689562034785;7.24561328048773;0.135409175827961]
Robot Arm 1 togoal: 25.1960255698966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-0.549743946414554;-9.08564794135202;1.55306008652033]
Robot Arm 1 end: [-9.38543821555365;1.72329384512727;0.106563545429225]
Robot Arm 1 togoal: 21.1258067317481
Mobile Platform 2 Pose: [-0.340658295352493;8.73748800600008;7.51826998417314]
Robot Arm 2 end: [8.30395379831682;7.18041417145479;0.135466399371272]
Robot Arm 1 togoal: 25.1420178011255

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-0.552794938991999;-9.08358016238173;1.62897729205999]
Robot Arm 1 end: [-9.3835555097163;1.80042547072146;0.106322765853799]
Robot Arm 1 togoal: 21.0939934184179
Mobile Platform 2 Pose: [-0.324763734280726;8.72662755849985;7.44997450831124]
Robot Arm 2 end: [8.2910235860928;7.11469406485722;0.135523293687465]
Robot Arm 1 togoal: 25.0877229429934

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-0.555857054190423;-9.08150482332564;1.7056000972716]
Robot Arm 1 end: [-9.38166424503065;1.87826594263479;0.106084712820198]
Robot Arm 1 togoal: 21.0621332793134
Mobile Platform 2 Pose: [-0.308857323992662;8.71575917040093;7.38114811834943]
Robot Arm 2 end: [8.2781052112574;7.04845247622786;0.135579900803223]
Robot Arm 1 togoal: 25.0331430467541

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-0.558930053764384;-9.07942208701897;1.78293862555626]
Robot Arm 1 end: [-9.37976453867431;1.95682540454311;0.105849400816661]
Robot Arm 1 togoal: 21.0302302023975
Mobile Platform 2 Pose: [-0.292939403106021;8.70488307292466;7.31179048031466]
Robot Arm 2 end: [8.26519890101262;6.98168883714298;0.135636262552391]
Robot Arm 1 togoal: 24.9782801517665

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-0.562013692522142;-9.07733212103784;1.86100313751857]
Robot Arm 1 end: [-9.37785651147624;2.03611413756915;0.105616844241861]
Robot Arm 1 togoal: 20.9982882751617
Mobile Platform 2 Pose: [-0.27701031080789;8.69399949781069;7.24190117470059]
Robot Arm 2 end: [8.25230487846886;6.91440249468336;0.135692421755764]
Robot Arm 1 togoal: 24.9231362836837

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-0.565107718116634;-9.07523509784184;1.9398040321603]
Robot Arm 1 end: [-9.37594028803248;2.1161425614745;0.105387057401188]
Robot Arm 1 togoal: 20.9663117920069
Mobile Platform 2 Pose: [-0.261070387510974;8.68310867748549;7.17147969319815]
Robot Arm 2 end: [8.23942336453142;6.84659270750088;0.135748421610313]
Robot Arm 1 togoal: 24.8677134545673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-0.568211870830669;-9.07313119492044;2.01935184801808]
Robot Arm 1 end: [-9.37401599682515;2.1969212357953;0.105160054502876]
Robot Arm 1 togoal: 20.9343052618635
Mobile Platform 2 Pose: [-0.245119974909244;8.67221084528819;7.1005254360266]
Robot Arm 2 end: [8.22655457967861;6.77825864217837;0.135804305223612]
Robot Arm 1 togoal: 24.8120136631968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-0.571325883356194;-9.0710205949435;2.09965726423972]
Robot Arm 1 end: [-9.37208377034484;2.27846086091583;0.104935849653965]
Robot Arm 1 togoal: 20.902273416056
Mobile Platform 2 Pose: [-0.229159416149134;8.66130623552716;7.0290377092887]
Robot Arm 2 end: [8.21369874202571;6.7093993717704;0.135860116204185]
Robot Arm 1 togoal: 24.756038894142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-0.574449480567552;-9.06890348591592;2.18073110159303]
Robot Arm 1 end: [-9.37014374521664;2.36077227907431;0.104714456856102]
Robot Arm 1 togoal: 20.8702212164182
Mobile Platform 2 Pose: [-0.213189055943794;8.65039508350814;6.95701572231972]
Robot Arm 2 end: [8.20085606890364;6.64001387217874;0.135915898190569]
Robot Arm 1 togoal: 24.6997911180709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-0.577582379288597;-9.06678006133633;2.26258432340109]
Robot Arm 1 end: [-9.36819606232958;2.44386647529449;0.104495890001155]
Robot Arm 1 togoal: 20.8381538636622
Mobile Platform 2 Pose: [-0.197209240512231;8.63947762564184;6.88445858494335]
Robot Arm 2 end: [8.18802677393075;6.57010102152637;0.135971695707575]
Robot Arm 1 togoal: 24.6432722907592

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-0.580724288053592;-9.06465052036016;2.3452280363972]
Robot Arm 1 end: [-9.36624086696977;2.52775457823634;0.104280162866641]
Robot Arm 1 togoal: 20.8060768060075
Mobile Platform 2 Pose: [-0.181220318400771;8.62855409981281;6.81136530451378]
Robot Arm 2 end: [8.17521107173505;6.49965959441967;0.136027552849739]
Robot Arm 1 togoal: 24.5864843544825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-0.583874906861765;-9.06251506796695;2.42867349149259]
Robot Arm 1 end: [-9.36427830895724;2.6124478609589;0.104067289110967]
Robot Arm 1 togoal: 20.7739957480738
Mobile Platform 2 Pose: [-0.165222640491759;8.61762474549648;6.73773478382227]
Robot Arm 2 end: [8.16240917585703;6.42868826097727;0.136083514006691]
Robot Arm 1 togoal: 24.5294292375669

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-0.587033926925455;-9.06037391513204;2.51293208444933]
Robot Arm 1 end: [-9.36230854278655;2.6979577415877;0.103857282268454]
Robot Arm 1 togoal: 20.7419166600419
Mobile Platform 2 Pose: [-0.149216559789459;8.60668980365151;6.66356581863422]
Robot Arm 2 end: [8.14962129853161;6.35718558493366;0.13613962375793]
Robot Arm 1 togoal: 24.4721088548118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-0.588988024274777;-9.05883378294243;2.59801535893614]
Robot Arm 1 end: [-9.35797814878731;2.78083075594375;0.102050027177061]
Robot Arm 1 togoal: 20.7089628327599
Mobile Platform 2 Pose: [-0.133202431901141;8.59574951695023;6.58885709501961]
Robot Arm 2 end: [8.13684765152905;6.28515002029802;0.136195926748329]
Robot Arm 1 togoal: 24.4145251078058

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-0.587286789481365;-9.05912896111435;2.68379508196813]
Robot Arm 1 end: [-9.36141347397838;2.87130368551634;0.103444965506955]
Robot Arm 1 togoal: 20.6807181966754
Mobile Platform 2 Pose: [-0.11718061517018;8.58480412994821;6.51360718706368]
Robot Arm 2 end: [8.12408844609804;6.21257990937949;0.136252467702873]
Robot Arm 1 togoal: 24.3566798855715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-0.585975923230257;-9.05921895477022;2.7705399390586]
Robot Arm 1 end: [-9.36168480904336;2.959286513924;0.103243638967205]
Robot Arm 1 togoal: 20.6508236672778
Mobile Platform 2 Pose: [-0.101151471228786;8.5738538893733;6.43781455453597]
Robot Arm 2 end: [8.11134389385025;6.13947347977625;0.136309291291138]
Robot Arm 1 togoal: 24.2985750653097

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986720e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-0.58297157884179;-9.06015469786177;2.85812904005431]
Robot Arm 1 end: [-9.36198598342476;3.05854384011356;0.105910559744481]
Robot Arm 1 togoal: 20.6172935412455
Mobile Platform 2 Pose: [-0.0851153648010698;8.56289904407411;6.36147754086675]
Robot Arm 2 end: [8.0986142064355;6.06582884306193;0.136366442069892]
Robot Arm 1 togoal: 24.2402125134599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-0.583651385496696;-9.05929877966066;2.93828751992576]
Robot Arm 1 end: [-9.35866611070956;3.13693339431877;0.104300841252909]
Robot Arm 1 togoal: 20.5880225695997
Mobile Platform 2 Pose: [-0.0690726640600811;8.55193984523027;6.28459437052875]
Robot Arm 2 end: [8.0858995942029;5.99164399270981;0.136423965086721]
Robot Arm 1 togoal: 24.1815940855357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-0.586172215777015;-9.0575301177993;3.01903059171751]
Robot Arm 1 end: [-9.35935254805516;3.22214544042106;0.105465376014073]
Robot Arm 1 togoal: 20.5603400210794
Mobile Platform 2 Pose: [-0.0530237408665681;8.54097654649244;6.20716314700544]
Robot Arm 2 end: [8.07320027044418;5.91691679951085;0.136481904489819]
Robot Arm 1 togoal: 24.122721628683

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-0.591336603358146;-9.05443093750953;3.10061348010497]
Robot Arm 1 end: [-9.35376912250453;3.30197726773232;0.103848024344312]
Robot Arm 1 togoal: 20.5290089261827
Mobile Platform 2 Pose: [-0.0369689707413864;8.53000940408415;6.12918185086243]
Robot Arm 2 end: [8.06051644666965;5.84164501315712;0.136540305017067]
Robot Arm 1 togoal: 24.0635969815475

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-0.599550244144201;-9.04981493362514;3.18281546231943]
Robot Arm 1 end: [-9.3495349501981;3.38646783301669;0.104514264656204]
Robot Arm 1 togoal: 20.4975545456304
Mobile Platform 2 Pose: [-0.0209087336907918;8.51903867719546;6.05064833719041]
Robot Arm 2 end: [8.04784833578532;5.76582625736594;0.136599211176667]
Robot Arm 1 togoal: 24.0042219759994

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-0.611728383669986;-9.0432218984227;3.26595045719701]
Robot Arm 1 end: [-9.34492488479863;3.47325553627635;0.104817909033944]
Robot Arm 1 togoal: 20.4653385950006
Mobile Platform 2 Pose: [-0.00484341372269326;8.50806462774396;5.97156033399544]
Robot Arm 2 end: [8.03519615372211;5.68945802677228;0.136658666688682]
Robot Arm 1 togoal: 23.944598438984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-0.626846402955948;-9.03514372792767;3.34982217655283]
Robot Arm 1 end: [-9.33678173175773;3.55856619118965;0.104608496583601]
Robot Arm 1 togoal: 20.4305986732126
Mobile Platform 2 Pose: [0.0112266003118506;8.49708752091593;5.89191544020653]
Robot Arm 2 end: [8.0225601142422;5.61253769023875;0.13671871594193]
Robot Arm 1 togoal: 23.8847281934292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-0.643229516932675;-9.02643205693258;3.43450360966048]
Robot Arm 1 end: [-9.32799646581087;3.64469757211608;0.104401937516052]
Robot Arm 1 togoal: 20.3952952026417
Mobile Platform 2 Pose: [0.0273009152431736;8.48610762490603;5.81171112317858]
Robot Arm 2 end: [8.00994043413418;5.53506248363056;0.136779402477472]
Robot Arm 1 togoal: 23.82461306032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-0.646932099544762;-9.0240597048569;3.52001069940458]
Robot Arm 1 end: [-9.32559324502219;3.73159228490951;0.104198244354926]
Robot Arm 1 togoal: 20.3661193350489
Mobile Platform 2 Pose: [0.0433791324768809;8.47512521152351;5.73094471731339]
Robot Arm 2 end: [7.99733733234807;5.45702950858127;0.136840769707748]
Robot Arm 1 togoal: 23.7642548601526

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-0.649881085563331;-9.02206406340558;3.6063059717996]
Robot Arm 1 end: [-9.3235657681314;3.81927542143923;0.103997446710667]
Robot Arm 1 togoal: 20.337390357353
Mobile Platform 2 Pose: [0.0594608481567161;8.46414055615321;5.64961342213729]
Robot Arm 2 end: [7.9847510291;5.3784357324906;0.136902860775225]
Robot Arm 1 togoal: 23.7036554153124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-0.65283004567659;-9.02006848413932;3.69339701854118]
Robot Arm 1 end: [-9.32153452000503;3.9077585496316;0.103799557754405]
Robot Arm 1 togoal: 20.3087557272924
Mobile Platform 2 Pose: [0.0755456531908244;8.45315393779054;5.56771430031879]
Robot Arm 2 end: [7.97218174658378;5.29927798553754;0.136965718456135]
Robot Arm 1 togoal: 23.6428165518299

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-0.655778482990703;-9.01807330641593;3.78129439740875]
Robot Arm 1 end: [-9.31949974962317;3.99705224944364;0.103604589486363]
Robot Arm 1 togoal: 20.2802234213863
Mobile Platform 2 Pose: [0.0916331326796578;8.44216563934413;5.48524427599908]
Robot Arm 2 end: [7.95962970891086;5.2195529594899;0.137029385231177]
Robot Arm 1 togoal: 23.5817401018342

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-0.658725888205215;-9.01607887805644;3.8700087684345]
Robot Arm 1 end: [-9.31746171260016;4.08716720297962;0.103412553724909]
Robot Arm 1 togoal: 20.2518017191343
Mobile Platform 2 Pose: [0.107722865583034;8.43117594795951;5.40220013277596]
Robot Arm 2 end: [7.94709514273299;5.13925720531268;0.137093903189853]
Robot Arm 1 togoal: 23.5204279058802

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-0.661671739322092;-9.01408555554342;3.95955089258492]
Robot Arm 1 end: [-9.3154206713505;4.17811419314935;0.103223462099639]
Robot Arm 1 togoal: 20.2234992118244
Mobile Platform 2 Pose: [0.123814424648751;8.42018515488837;5.31857851214896]
Robot Arm 2 end: [7.93457827753087;5.05838713106086;0.137159313891316]
Robot Arm 1 togoal: 23.4588818153447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-0.664615501350869;-9.01209370422178;4.04993163026421]
Robot Arm 1 end: [-9.31337689525754;4.269904102147;0.103037326044195]
Robot Arm 1 togoal: 20.1953248114902
Mobile Platform 2 Pose: [0.139907376376761;8.40919355575967;5.23437591197024]
Robot Arm 2 end: [7.92207934559016;4.97693900101942;0.137225658383607]
Robot Arm 1 togoal: 23.397103695509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-0.667556626010093;-9.01010369850222;4.14116193962975]
Robot Arm 1 end: [-9.31133066084461;4.36254790974047;0.102854156788801]
Robot Arm 1 togoal: 20.1672877600104
Mobile Platform 2 Pose: [0.15600128003102;8.39820145102823;5.14958868456429]
Robot Arm 2 end: [7.90959858103247;4.89490893446404;0.137292977725503]
Robot Arm 1 togoal: 23.3350954277712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-0.670494551425283;-9.00811592206694;4.23325287470936]
Robot Arm 1 end: [-9.30928225194869;4.45605669136121;0.102673965352521]
Robot Arm 1 togoal: 20.1393976383451
Mobile Platform 2 Pose: [0.172095688414546;8.38720914564086;5.06421303505796]
Robot Arm 2 end: [7.89713622311366;4.8122929013446;0.137361311694067]
Robot Arm 1 togoal: 23.2728589134143

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-0.673428701823672;-9.00613076807759;4.32621558330998]
Robot Arm 1 end: [-9.30723195989635;4.5504416159838;0.102496762535224]
Robot Arm 1 togoal: 20.1116643758974
Mobile Platform 2 Pose: [0.188190146625066;8.37621694972024;4.97824502024077]
Robot Arm 2 end: [7.88469251306879;4.72908672405691;0.137430700087858]
Robot Arm 1 togoal: 23.2103960763161

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-0.676358487225996;-9.00414863938517;4.42006130470681]
Robot Arm 1 end: [-9.30518008368195;4.6457139437844;0.102322558909242]
Robot Arm 1 togoal: 20.0840982599925
Mobile Platform 2 Pose: [0.204284192306831;8.36522517848883;4.89168054651213]
Robot Arm 2 end: [7.87226769603174;4.64528607397826;0.137501182009298]
Robot Arm 1 togoal: 23.1477088664659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-0.679283303135646;-9.00216994874179;4.51480136710175]
Robot Arm 1 end: [-9.30312693014788;4.74188502356692;0.102151364810719]
Robot Arm 1 togoal: 20.0567099454614
Mobile Platform 2 Pose: [0.220377355202847;8.35423415252099;4.80451536845003]
Robot Arm 2 end: [7.85986202041041;4.56088647059879;0.13757279616142]
Robot Arm 1 togoal: 23.0847992632833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-0.682202530225534;-9.00019511901389;4.61044718483962]
Robot Arm 1 end: [-9.30107281416671;4.83896628994517;0.101983190330632]
Robot Arm 1 togoal: 20.0295104643177
Mobile Platform 2 Pose: [0.236469157225784;8.34324419780828;4.71674508727962]
Robot Arm 2 end: [7.84747574063915;4.47588327750044;0.137645579896516]
Robot Arm 1 togoal: 23.0216692797925

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-0.685115534023032;-8.99822458339678;4.70701025537032]
Robot Arm 1 end: [-9.29901805882503;4.93696926026928;0.101818045305488]
Robot Arm 1 togoal: 20.0025112355135
Mobile Platform 2 Pose: [0.25255911145328;8.33225564629237;4.62836515005016]
Robot Arm 2 end: [7.83510911352705;4.39027170541381;0.13771957063055]
Robot Arm 1 togoal: 22.9583209663365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-0.688021664593421;-8.99625878563024;4.80450215594478]
Robot Arm 1 end: [-9.29696299560887;5.03590553128396;0.101655939307669]
Robot Arm 1 togoal: 19.9757240747578
Mobile Platform 2 Pose: [0.26864672251452;8.32126883568099;4.53937084744468]
Robot Arm 2 end: [7.82276240090543;4.30404680736067;0.13779480485177]
Robot Arm 1 togoal: 22.8947564144757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-0.690920256222279;-8.99429818021471;4.9029345400322]
Robot Arm 1 end: [-9.29490796459038;5.13578677550631;0.101496881635432]
Robot Arm 1 togoal: 19.9491612043815
Mobile Platform 2 Pose: [0.284731485906147;8.3102841099203;4.44975731269238]
Robot Arm 2 end: [7.81043586928765;4.21720347834803;0.137871318377185]
Robot Arm 1 togoal: 22.8309777615565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-0.693810627097322;-8.99234323262796;5.00231913344603]
Robot Arm 1 end: [-9.29285331461555;5.2366247373103;0.101340881302544]
Robot Arm 1 togoal: 19.9228352632294
Mobile Platform 2 Pose: [0.300812888019562;8.29930181913801;4.35951952017003]
Robot Arm 2 end: [7.79812979008491;4.12973645304406;0.137949146348475]
Robot Arm 1 togoal: 22.7669871946416

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-0.696692078990212;-8.99039441954165;5.10266773016552]
Robot Arm 1 end: [-9.29079940349274;5.33843122870487;0.101187947027551]
Robot Arm 1 togoal: 19.896759316559
Mobile Platform 2 Pose: [0.316890405405943;8.28832232019418;4.26865228412144]
Robot Arm 2 end: [7.78584443973294;4.04164030686463;0.138028322914861]
Robot Arm 1 togoal: 22.7027869567012

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-0.699563896938935;-8.98845222903759;5.20399218783979]
Robot Arm 1 end: [-9.28874659818174;5.44121812479257;0.101038087222659]
Robot Arm 1 togoal: 19.8709468659228
Mobile Platform 2 Pose: [0.332963504781991;8.27734597664098;4.17715025746223]
Robot Arm 2 end: [7.77358010138472;3.95290945030355;0.138108881250988]
Robot Arm 1 togoal: 22.6383793500114

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-0.702425348931353;-8.98651716082305;5.30630442296106]
Robot Arm 1 end: [-9.28669527498289;5.54499735889509;0.100891309982241]
Robot Arm 1 togoal: 19.8454118590107
Mobile Platform 2 Pose: [0.349031642491446;8.2663731591134;4.08500793080877]
Robot Arm 2 end: [7.76133706199758;3.86353813296701;0.138190854283932]
Robot Arm 1 togoal: 22.5737667421189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-0.705275685590625;-8.98458972644494;5.40961640569332]
Robot Arm 1 end: [-9.28464581972618;5.64978091733225;0.10074762307093]
Robot Arm 1 togoal: 19.8201686994249
Mobile Platform 2 Pose: [0.365094264492571;8.25540424530374;3.99221963061273]
Robot Arm 2 end: [7.7491156146197;3.77352043877633;0.138274273949163]
Robot Arm 1 togoal: 22.5089515706976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-0.708114139863207;-8.98267044950202;5.51394015434308]
Robot Arm 1 end: [-9.28259862795971;5.75558083384074;0.100607033911321]
Robot Arm 1 togoal: 19.7952322563583
Mobile Platform 2 Pose: [0.381150805523803;8.24443962055367;3.89877951835425]
Robot Arm 2 end: [7.73691606009706;3.68285028430561;0.138359170755949]
Robot Arm 1 togoal: 22.4439363501224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-0.710939926710223;-8.98075986585496;5.6192877294582]
Robot Arm 1 end: [-9.28055410513726;5.86240918361869;0.10046954957126]
Robot Arm 1 togoal: 19.7706178741466
Mobile Platform 2 Pose: [0.397200689391527;8.2334796777235;3.80468158978765]
Robot Arm 2 end: [7.72473870423777;3.59152141993448;0.138445574851687]
Robot Arm 1 togoal: 22.3787236763078

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-0.713752242803035;-8.97885852383341;5.72567122754115]
Robot Arm 1 end: [-9.27851266680443;5.97027807698248;0.100335176750707]
Robot Arm 1 togoal: 19.7463413816586
Mobile Platform 2 Pose: [0.413243328060825;8.22252481766444;3.70991967332404]
Robot Arm 2 end: [7.71258385830769;3.49952743009436;0.138533515594809]
Robot Arm 1 togoal: 22.3133162338984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-0.716550266223905;-8.97696698443959;5.83310277436308]
Robot Arm 1 end: [-9.27647473878299;6.07919965262182;0.100203921768188]
Robot Arm 1 togoal: 19.722419101491
Mobile Platform 2 Pose: [0.429278121407616;8.21157544949073;3.61448742907973]
Robot Arm 2 end: [7.70045184345448;3.40686172544289;0.13862302059789]
Robot Arm 1 togoal: 22.2477168019727

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-0.717145451886205;-8.97617967210983;5.94159451353571]
Robot Arm 1 end: [-9.27911706855839;6.18845240208458;0.10018994409869]
Robot Arm 1 togoal: 19.7035771181099
Mobile Platform 2 Pose: [0.445304457344234;8.2006319905284;3.5183783484061]
Robot Arm 2 end: [7.68834298483469;3.31351754998733;0.138714117473447]
Robot Arm 1 togoal: 22.1819282607262

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999832e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-0.710561899776553;-8.97898431099656;6.05424220474807]
Robot Arm 1 end: [-9.2852221656423;6.30340584820711;0.100953231596599]
Robot Arm 1 togoal: 19.6875883522006
Mobile Platform 2 Pose: [0.461321710581196;8.18969486706329;3.42158575223103]
Robot Arm 2 end: [7.6762576177273;3.21948797339646;0.138806831885353]
Robot Arm 1 togoal: 22.1159535991786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999470e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-0.703188529099007;-8.98219941534039;6.1688347520909]
Robot Arm 1 end: [-9.2909675034578;6.42285658200861;0.102689671967578]
Robot Arm 1 togoal: 19.6710189476536
Mobile Platform 2 Pose: [0.477329242855776;8.17876451420801;3.32410279093562]
Robot Arm 2 end: [7.66419608311042;3.12476589473317;0.138901189241597]
Robot Arm 1 togoal: 22.0497959214326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-0.695422978646286;-8.98564375552575;6.28204234901956]
Robot Arm 1 end: [-9.29538258028552;6.54431812701268;0.105446718764393]
Robot Arm 1 togoal: 19.6534301641952
Mobile Platform 2 Pose: [0.493326402671052;8.16784137617391;3.22592244299596]
Robot Arm 2 end: [7.65215873028978;3.02934404054356;0.138997213400815]
Robot Arm 1 togoal: 21.9834584555834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992904e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-0.68785324276262;-8.98904241445397;6.3898378885881]
Robot Arm 1 end: [-9.29772807735998;6.66308367682061;0.108722909680009]
Robot Arm 1 togoal: 19.6349648860259
Mobile Platform 2 Pose: [0.509312524289509;8.15692590682309;3.12703751458481]
Robot Arm 2 end: [7.64014591720974;2.93321496198802;0.139094927330409]
Robot Arm 1 togoal: 21.9169445603924

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989065e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-0.680403464644549;-8.99244265284296;6.4894564120967]
Robot Arm 1 end: [-9.29796272991458;6.77603542895917;0.112509286870001]
Robot Arm 1 togoal: 19.6160434127138
Mobile Platform 2 Pose: [0.525286927749138;8.14601856974173;3.02744063863868]
Robot Arm 2 end: [7.62815801021959;2.83637103570216;0.13919435277476]
Robot Arm 1 togoal: 21.8502577340906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984304e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-0.672839914598358;-8.99596586311363;6.57867284193224]
Robot Arm 1 end: [-9.29556015138991;6.88146799671301;0.117153650406796]
Robot Arm 1 togoal: 19.5962921999213
Mobile Platform 2 Pose: [0.541248918340388;8.13511983857649;2.92712427460493]
Robot Arm 2 end: [7.61619538481957;2.73880446280743;0.139295510195185]
Robot Arm 1 togoal: 21.7834016231379

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-0.669998996857068;-8.99719672220609;6.65478362802843]
Robot Arm 1 end: [-9.29567248196493;6.95911669108265;0.117258556039794]
Robot Arm 1 togoal: 19.5841884659845
Mobile Platform 2 Pose: [0.557197786024558;8.12423019741174;2.82608070775409]
Robot Arm 2 end: [7.60425842426754;2.64050726988187;0.139398419413861]
Robot Arm 1 togoal: 21.7163800306545

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-0.669545868465761;-8.99724481809903;6.73132373659671]
Robot Arm 1 end: [-9.29474796030765;7.03729139503515;0.117112060696653]
Robot Arm 1 togoal: 19.5713015018381
Mobile Platform 2 Pose: [0.573132805478005;8.11335014081961;2.72430204859278]
Robot Arm 2 end: [7.59234752218038;2.5414713058853;0.139503098643267]
Robot Arm 1 togoal: 21.649196926121

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-0.672251770436183;-8.99571951861942;6.80823771317583]
Robot Arm 1 end: [-9.29226413551036;7.11588115231709;0.116904116219706]
Robot Arm 1 togoal: 19.5571235679664
Mobile Platform 2 Pose: [0.589053234974762;8.10248017450382;2.62178023272319]
Robot Arm 2 end: [7.58046308247307;2.44168824225957;0.139609564653831]
Robot Arm 1 togoal: 21.581856455056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-0.678634310586029;-8.99236067394679;6.88552551001787]
Robot Arm 1 end: [-9.28792535842199;7.19487291004172;0.116682778664552]
Robot Arm 1 togoal: 19.5413640639802
Mobile Platform 2 Pose: [0.604958316193535;8.091620815489;2.51850702071307]
Robot Arm 2 end: [7.5686055172557;2.3411495755584;0.139717833725253]
Robot Arm 1 togoal: 21.5143629487401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995610e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-0.68048537925886;-8.99127189400675;6.96320257898745]
Robot Arm 1 end: [-9.3349579627715;7.21299193085102;0.125181010639861]
Robot Arm 1 togoal: 19.5845940486572
Mobile Platform 2 Pose: [0.620847274076223;8.08077259228559;2.41447399746956]
Robot Arm 2 end: [7.5567752509172;2.23984662233895;0.139827920035492]
Robot Arm 1 togoal: 21.4467209350072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','2.522427e-13','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-0.682861352165854;-8.98981070959528;7.05511234872542]
Robot Arm 1 end: [-9.37019916417359;7.24811713422941;0.135167308024062]
Robot Arm 1 togoal: 19.6137198719881
Mobile Platform 2 Pose: [0.845615342003676;7.96548802783405;2.30967255113406]
Robot Arm 2 end: [7.44115492833085;2.13585767321952;0.139939837304238]
Robot Arm 1 togoal: 21.2927277226678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.934827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-0.685965691392889;-8.98786456554901;7.15989720718847]
Robot Arm 1 end: [-9.39955001341798;7.28897820362181;0.14686841384453]
Robot Arm 1 togoal: 19.6362017080599
Mobile Platform 2 Pose: [0.865295238068614;7.95275492898646;2.20349913142181]
Robot Arm 2 end: [7.42770308597561;2.01459195692014;0.145169435964249]
Robot Arm 1 togoal: 21.2124530520435

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996136e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999676e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-0.690049595287528;-8.98526646731581;7.2780295619431]
Robot Arm 1 end: [-9.42214889154679;7.34079574553102;0.159598657115034]
Robot Arm 1 togoal: 19.6505635656794
Mobile Platform 2 Pose: [0.881726894110113;7.94166450292194;2.09525165618737]
Robot Arm 2 end: [7.44742198640463;1.91464183643892;0.117718939119238]
Robot Arm 1 togoal: 21.1740506705288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-0.694935153803919;-8.98208489378955;7.40914758748157]
Robot Arm 1 end: [-9.43531229767419;7.42449978772481;0.169680358547107]
Robot Arm 1 togoal: 19.6517335170445
Mobile Platform 2 Pose: [0.897988979524712;7.93072970296768;1.99423920908746]
Robot Arm 2 end: [7.45455045034006;1.81486296840518;0.104222117875294]
Robot Arm 1 togoal: 21.1248568741409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997573e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999528e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-0.700401628915096;-8.97848427117627;7.54939568303482]
Robot Arm 1 end: [-9.44202602829302;7.52231296730639;0.178177306938988]
Robot Arm 1 togoal: 19.6452152419311
Mobile Platform 2 Pose: [0.914203081238159;7.91987262955021;1.89720739916004]
Robot Arm 2 end: [7.45182634576791;1.71328727165666;0.100147169992079]
Robot Arm 1 togoal: 21.0662327974352

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997670e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-0.706481234424546;-8.97445227989426;7.6973796180806]
Robot Arm 1 end: [-9.44400399283205;7.62989056952693;0.18571635233546]
Robot Arm 1 togoal: 19.6333820928898
Mobile Platform 2 Pose: [0.930554122904689;7.90898614227061;1.80167040562152]
Robot Arm 2 end: [7.43378416396811;1.60332490494751;0.110227712425072]
Robot Arm 1 togoal: 20.9896226162594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997809e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999335e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-0.713187008539004;-8.96997953235077;7.85232621673486]
Robot Arm 1 end: [-9.44192308424738;7.74698024047036;0.192243323081528]
Robot Arm 1 togoal: 19.6170860618257
Mobile Platform 2 Pose: [0.947102397269057;7.89801508607269;1.70343550844966]
Robot Arm 2 end: [7.41684230361175;1.49122046288944;0.119662683747301]
Robot Arm 1 togoal: 20.9131128679958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998838e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-0.7205538741306;-8.96504864763668;8.01351914685053]
Robot Arm 1 end: [-9.44186422231748;7.87687688414071;0.19946439589476]
Robot Arm 1 togoal: 19.6020456217468
Mobile Platform 2 Pose: [0.961707851239143;7.88803193384069;1.60266032270292]
Robot Arm 2 end: [7.39519357331707;1.40663282981087;0.122463461133442]
Robot Arm 1 togoal: 20.8484996203045

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.141323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-0.728347397428086;-8.95981663770298;8.17958201278199]
Robot Arm 1 end: [-9.44055887116872;8.01436197789841;0.205795338737789]
Robot Arm 1 togoal: 19.5859094983919
Mobile Platform 2 Pose: [0.976484822820867;7.87792873900569;1.49984079515642]
Robot Arm 2 end: [7.37940601489179;1.34502104240935;0.11679664330548]
Robot Arm 1 togoal: 20.8020442012346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.454867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-0.736572074965107;-8.95429203061501;8.35006814353579]
Robot Arm 1 end: [-9.43372467845153;8.16613613200135;0.20960794744339]
Robot Arm 1 togoal: 19.5640604343709
Mobile Platform 2 Pose: [0.991494455329623;7.86767493825562;1.3981338803236]
Robot Arm 2 end: [7.38847217824493;1.2643516473481;0.096548318195991]
Robot Arm 1 togoal: 20.7671289619559

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-0.745109401259159;-8.9485341610469;8.52333139441156]
Robot Arm 1 end: [-9.43272972439074;8.33973566362132;0.212245845477548]
Robot Arm 1 togoal: 19.547386005419
Mobile Platform 2 Pose: [1.00645963911309;7.8574524398585;1.30110325450435]
Robot Arm 2 end: [7.29998125803622;1.13543006093284;0.160009939045354]
Robot Arm 1 togoal: 20.6189099643106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-0.753538268465048;-8.94283286445012;8.69704477345405]
Robot Arm 1 end: [-9.43174982488256;8.51439292715609;0.214320442058008]
Robot Arm 1 togoal: 19.5322129083944
Mobile Platform 2 Pose: [1.02232306623836;7.84662530950134;1.17886234018333]
Robot Arm 2 end: [7.2884896987937;1.01586387341634;0.159945654779638]
Robot Arm 1 togoal: 20.544916691814

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-0.761842112108081;-8.93719910666483;8.87134524257741]
Robot Arm 1 end: [-9.43017693765298;8.69023729642679;0.215406935042629]
Robot Arm 1 togoal: 19.5179872789035
Mobile Platform 2 Pose: [1.03815421518126;7.83582033022934;1.05587102447848]
Robot Arm 2 end: [7.27703409255979;0.895538612823478;0.159883997085375]
Robot Arm 1 togoal: 20.4709869198432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999818e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-0.770023673600815;-8.93163115811804;9.04647983193407]
Robot Arm 1 end: [-9.42801366837243;8.86726065072043;0.215747326193436]
Robot Arm 1 togoal: 19.5047301461556
Mobile Platform 2 Pose: [1.05395201798676;7.8250382303843;0.932119334711062]
Robot Arm 2 end: [7.26561505433093;0.774444010597376;0.159824976300122]
Robot Arm 1 togoal: 20.397129807573

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999799e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-0.778095362321659;-8.92612141832448;9.22263513072937]
Robot Arm 1 end: [-9.42519430919941;9.04566515596114;0.215259063746531]
Robot Arm 1 togoal: 19.4924059145429
Mobile Platform 2 Pose: [1.06971538014719;7.81427975640978;0.807597185801817]
Robot Arm 2 end: [7.25423321536294;0.65256968277515;0.1597686013627]
Robot Arm 1 togoal: 20.3233549301573

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.199798e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-0.78607370926623;-8.92066001424622;9.40003172280103]
Robot Arm 1 end: [-9.42161292229865;9.22567668255383;0.213858715542978]
Robot Arm 1 togoal: 19.4809456505856
Mobile Platform 2 Pose: [1.09458916182343;7.79897268167081;0.682294386363326]
Robot Arm 2 end: [7.24301808709754;0.46662618699959;0.172371713488516]
Robot Arm 1 togoal: 20.216100878892

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999140e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-0.794183584980264;-8.91513243298194;9.57892573337317]
Robot Arm 1 end: [-9.39135301604949;9.4234710421901;0.174557602441741]
Robot Arm 1 togoal: 19.4465900497282
Mobile Platform 2 Pose: [1.12065312108222;7.78315571850017;0.552124646777316]
Robot Arm 2 end: [7.24313110034034;0.266614052454081;0.186624534850308]
Robot Arm 1 togoal: 20.1124213392512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986931e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-0.801481039272784;-8.91014721615352;9.74924216523786]
Robot Arm 1 end: [-9.39730771052916;9.58415911815675;0.195697448174909]
Robot Arm 1 togoal: 19.446976549321
Mobile Platform 2 Pose: [1.13535330929767;7.77314049642497;0.41846547692662]
Robot Arm 2 end: [7.25787543450955;0.0677312548279861;0.200949429785787]
Robot Arm 1 togoal: 20.0233865926923

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-0.809996817756121;-8.90432791472311;9.93916039645394]
Robot Arm 1 end: [-9.3721944674643;9.77754711925724;0.182049874147359]
Robot Arm 1 togoal: 19.4196856619495
Mobile Platform 2 Pose: [1.14925748164799;7.76366072584157;0.283054345595873]
Robot Arm 2 end: [7.29088186307398;-0.059188101239158;0.154642372346198]
Robot Arm 1 togoal: 19.9915299558581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981221e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997960e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-0.819229889519932;-8.89801769957442;10.1385493768449]
Robot Arm 1 end: [-9.34608730679266;9.98444266310028;0.167020580857491]
Robot Arm 1 togoal: 19.3934163153436
Mobile Platform 2 Pose: [1.17395452779902;7.74887887433863;0.156946452810479]
Robot Arm 2 end: [7.26499543148066;-0.202759364887276;0.177567145960264]
Robot Arm 1 togoal: 19.8965022841872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979871e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-0.829167802363211;-8.89122682941003;10.3494255332069]
Robot Arm 1 end: [-9.3246055278554;10.2042531688011;0.154905868746755]
Robot Arm 1 togoal: 19.373908003798
Mobile Platform 2 Pose: [1.1919659759842;7.73746536670297;0.0256175319786941]
Robot Arm 2 end: [7.24770402259045;-0.336832235974803;0.184981146970725]
Robot Arm 1 togoal: 19.8152731917132

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-0.839666086688218;-8.88405396547359;10.5716435667538]
Robot Arm 1 end: [-9.30741883818136;10.4345335891863;0.146091483303482]
Robot Arm 1 togoal: 19.3611883690639
Mobile Platform 2 Pose: [1.20876185436882;7.72666424558546;-0.108880195179483]
Robot Arm 2 end: [7.24076035375382;-0.459323285183548;0.17487932922327]
Robot Arm 1 togoal: 19.7504524153879

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004258e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-0.850603941846482;-8.87657860702941;10.8039620541881]
Robot Arm 1 end: [-9.28969592535295;10.6789034812245;0.134909391556082]
Robot Arm 1 togoal: 19.3513442944983
Mobile Platform 2 Pose: [1.22282157956991;7.71724463385809;-0.24264735485156]
Robot Arm 2 end: [7.23335761643051;-0.574504633048624;0.164047948118501]
Robot Arm 1 togoal: 19.6893345880279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004725e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-0.845005150342629;-8.87729442848213;11.0487149175951]
Robot Arm 1 end: [-9.284850238943;10.9379439859903;0.125883343973016]
Robot Arm 1 togoal: 19.3579838346673
Mobile Platform 2 Pose: [1.2353104514884;7.70861504037599;-0.375632533620536]
Robot Arm 2 end: [7.22640378880608;-0.68361397182811;0.152896954660877]
Robot Arm 1 togoal: 19.6320081006395

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005112e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-0.836059489952534;-8.87996500065893;11.3051366825703]
Robot Arm 1 end: [-9.2837974566091;11.2093389868764;0.117168676470668]
Robot Arm 1 togoal: 19.3726117887503
Mobile Platform 2 Pose: [1.24630804481182;7.70072474606169;-0.507364648529459]
Robot Arm 2 end: [7.22078649976131;-0.782756844344808;0.140614805566387]
Robot Arm 1 togoal: 19.5810756176939

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005403e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-0.821351935101899;-8.88436055671294;11.5634760905475]
Robot Arm 1 end: [-9.28619688368025;11.4903474898884;0.106416073824903]
Robot Arm 1 togoal: 19.3953492765892
Mobile Platform 2 Pose: [1.25593619566808;7.69349639514298;-0.636771924610402]
Robot Arm 2 end: [7.21758070988206;-0.869393901166569;0.127953679040639]
Robot Arm 1 togoal: 19.5385166091414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005591e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-0.802122846424134;-8.89040621219376;11.8343000559399]
Robot Arm 1 end: [-9.2923892253116;11.7883612984993;0.0961692315683303]
Robot Arm 1 togoal: 19.4274203350203
Mobile Platform 2 Pose: [1.26464560228971;7.68668706208934;-0.762326356349847]
Robot Arm 2 end: [7.21807426775315;-0.944222964951884;0.116573635699308]
Robot Arm 1 togoal: 19.5049120123909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990092e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005648e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-0.789790627564515;-8.89466531526153;12.1270604575485]
Robot Arm 1 end: [-9.29920485245215;12.1018025390122;0.0904508905722272]
Robot Arm 1 togoal: 19.4659536782659
Mobile Platform 2 Pose: [1.27266725680059;7.68017102384786;-0.882472687109078]
Robot Arm 2 end: [7.22388727681788;-1.00649041474053;0.107466124376882]
Robot Arm 1 togoal: 19.4818783899863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005543e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-0.783603658948545;-8.89544329113406;12.4377207004767]
Robot Arm 1 end: [-9.30589557823654;12.4244093079267;0.0890937906280493]
Robot Arm 1 togoal: 19.5101440272124
Mobile Platform 2 Pose: [1.28015200297975;7.67386966424307;-0.995591792157912]
Robot Arm 2 end: [7.23636511401626;-1.05624391992935;0.101718762775436]
Robot Arm 1 togoal: 19.4704240938356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977278e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005265e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-0.777169329761398;-8.89630928149381;12.753259953888]
Robot Arm 1 end: [-9.31094039530402;12.7518590632749;0.0874994620560065]
Robot Arm 1 togoal: 19.5586316251265
Mobile Platform 2 Pose: [1.28730190260208;7.66768681507155;-1.10023930196649]
Robot Arm 2 end: [7.25588491127356;-1.09569769682505;0.100162956109217]
Robot Arm 1 togoal: 19.4697442582777

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979313e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004805e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-0.770754992989283;-8.89713380848819;13.0714869476739]
Robot Arm 1 end: [-9.3170397764578;13.0821819146224;0.0865816762961948]
Robot Arm 1 togoal: 19.6139090190471
Mobile Platform 2 Pose: [1.2941859791018;7.66160164219417;-1.19557249281235]
Robot Arm 2 end: [7.28206477241865;-1.12692239925477;0.102913116258589]
Robot Arm 1 togoal: 19.4785071121162

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978135e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004224e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-0.763848482792172;-8.90102916411694;13.3905461408212]
Robot Arm 1 end: [-9.32957030082519;13.4164018786057;0.0866324754206132]
Robot Arm 1 togoal: 19.6815014672726
Mobile Platform 2 Pose: [1.30094946704914;7.65555987443722;-1.28140477158726]
Robot Arm 2 end: [7.31335485831811;-1.15334767064823;0.109387817599336]
Robot Arm 1 togoal: 19.4937980609212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003564e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-0.757668634710008;-8.90414992437634;13.7045009822993]
Robot Arm 1 end: [-9.34174527710303;13.7453998123469;0.0876729351126697]
Robot Arm 1 togoal: 19.7531151111313
Mobile Platform 2 Pose: [1.30759838023984;7.649579001914;-1.35833110236463]
Robot Arm 2 end: [7.34814945622604;-1.17708598026604;0.118643174298606]
Robot Arm 1 togoal: 19.5132956217489

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983138e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002804e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-0.754613757017051;-8.90514565143057;14.0083298976591]
Robot Arm 1 end: [-9.35148174026438;14.0633864248316;0.089527591499573]
Robot Arm 1 togoal: 19.8252381584078
Mobile Platform 2 Pose: [1.31389784726924;7.64379471025696;-1.42739401923751]
Robot Arm 2 end: [7.38587482417757;-1.19823448460012;0.129921677530411]
Robot Arm 1 togoal: 19.5364996478207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993496e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-0.755631076755702;-8.90339412684106;14.2981744911849]
Robot Arm 1 end: [-9.35816259727078;14.366535411371;0.0920779185543865]
Robot Arm 1 togoal: 19.8959018130864
Mobile Platform 2 Pose: [1.32016031628879;7.63806483049135;-1.48974703689272]
Robot Arm 2 end: [7.42398364913632;-1.22036842025622;0.141939893309321]
Robot Arm 1 togoal: 19.5596253455752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-0.764434859822219;-8.89723973265365;14.5481026738756]
Robot Arm 1 end: [-9.25255281809634;14.743927330508;0.0818671267081644]
Robot Arm 1 togoal: 19.8805618622241
Mobile Platform 2 Pose: [1.32633342103214;7.63242580789597;-1.54674239739855]
Robot Arm 2 end: [7.46147735672868;-1.24388162533676;0.154162026080781]
Robot Arm 1 togoal: 19.5816507607916

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000071e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-0.773502262874353;-8.89100094730425;14.825228424373]
Robot Arm 1 end: [-9.1539124427814;15.1086449203253;0.0889047844805627]
Robot Arm 1 togoal: 19.8751480873481
Mobile Platform 2 Pose: [1.33196631281205;7.62710885521896;-1.59956386460584]
Robot Arm 2 end: [7.49922955867378;-1.26699194474543;0.166852289426935]
Robot Arm 1 togoal: 19.6041368303313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999652e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-0.783107437219325;-8.88443523129802;15.1240238735483]
Robot Arm 1 end: [-9.05521869461043;15.4740947024207;0.10264601482985]
Robot Arm 1 togoal: 19.8765892531327
Mobile Platform 2 Pose: [1.34512976180893;7.61808138683301;-1.64913177345737]
Robot Arm 2 end: [7.53145041643318;-1.2928485059001;0.178938833228075]
Robot Arm 1 togoal: 19.620585338374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998805e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-0.792564187396688;-8.87800550338894;15.441456915532]
Robot Arm 1 end: [-8.95715477934315;15.8363669267346;0.117908652106311]
Robot Arm 1 togoal: 19.8848073051259
Mobile Platform 2 Pose: [1.3579151535533;7.609298145164;-1.69622982251661]
Robot Arm 2 end: [7.59709823918355;-1.32891020929761;0.188598726470525]
Robot Arm 1 togoal: 19.662679312637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-0.802177341494136;-8.87149730441611;15.7737308693693]
Robot Arm 1 end: [-8.87197529493569;16.1885379461946;0.127518347390298]
Robot Arm 1 togoal: 19.9095767046995
Mobile Platform 2 Pose: [1.37009192903491;7.6009385848855;-1.74150050380792]
Robot Arm 2 end: [7.64510167765046;-1.37717051545854;0.195559582904344]
Robot Arm 1 togoal: 19.6839933406149

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.950328e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-0.812480583948709;-8.86454715504283;16.1132607976309]
Robot Arm 1 end: [-8.79626741422317;16.5191304246531;0.125994655604367]
Robot Arm 1 togoal: 19.9435395582509
Mobile Platform 2 Pose: [1.3816835130971;7.59298260249738;-1.78756496031393]
Robot Arm 2 end: [7.68572583605122;-1.43499753985388;0.201831774136052]
Robot Arm 1 togoal: 19.6947802805314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-0.824457441586946;-8.85650516545913;16.4506381513649]
Robot Arm 1 end: [-8.7419167376681;16.8829484141217;0.148551771124411]
Robot Arm 1 togoal: 20.0129467099198
Mobile Platform 2 Pose: [1.39271438919476;7.58542606559602;-1.83574810097683]
Robot Arm 2 end: [7.69477813826031;-1.49700364589832;0.200845461255359]
Robot Arm 1 togoal: 19.6761004359677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964488e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-0.836367240757145;-8.84855199921481;16.794485940911]
Robot Arm 1 end: [-8.69476976534258;17.2509790187397;0.173002848165657]
Robot Arm 1 togoal: 20.0969701604676
Mobile Platform 2 Pose: [1.40353991209891;7.57801099826524;-1.88579312995538]
Robot Arm 2 end: [7.69455897672456;-1.54273831466943;0.206740123809981]
Robot Arm 1 togoal: 19.6557872296458

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966976e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-0.84720661220736;-8.84151148304663;17.1435122109022]
Robot Arm 1 end: [-8.64799966795757;17.6048675370826;0.183156972513114]
Robot Arm 1 togoal: 20.1834517820643
Mobile Platform 2 Pose: [1.40856402491846;7.5735066075054;-1.93681633212522]
Robot Arm 2 end: [7.69881128785227;-1.59694656505227;0.206962044197311]
Robot Arm 1 togoal: 19.6363419144932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995796e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-0.856240702794639;-8.83597677350931;17.4943553740971]
Robot Arm 1 end: [-8.60647880345603;17.9559422684315;0.189838492801703]
Robot Arm 1 togoal: 20.2797944220428
Mobile Platform 2 Pose: [1.4149840266586;7.56832071800444;-1.9879969027757]
Robot Arm 2 end: [7.70027804910171;-1.6504747798371;0.207592904573275]
Robot Arm 1 togoal: 19.6147763845517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995729e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.761652e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-0.861917525182029;-8.83209142535227;17.8462057178454]
Robot Arm 1 end: [-8.57168647557728;18.306044335851;0.194529452228903]
Robot Arm 1 togoal: 20.3876785135132
Mobile Platform 2 Pose: [1.42564707121234;7.56101954062989;-2.03984695051252]
Robot Arm 2 end: [7.69857897512815;-1.70429062616216;0.207896261190907]
Robot Arm 1 togoal: 19.5903734824761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.512224e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-0.866994634115559;-8.82831563886335;18.1982359757134]
Robot Arm 1 end: [-8.55057900251076;18.6706279247536;0.199824255092806]
Robot Arm 1 togoal: 20.5194852588178
Mobile Platform 2 Pose: [1.43630739969658;7.55372182293218;-2.0923176168226]
Robot Arm 2 end: [7.69408734862551;-1.75664533367964;0.207943224614841]
Robot Arm 1 togoal: 19.5641953923723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.100966e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-0.867747995663667;-8.82771294961172;18.5515633579587]
Robot Arm 1 end: [-8.53387484539555;19.0310374257183;0.206762472506119]
Robot Arm 1 togoal: 20.6589296443011
Mobile Platform 2 Pose: [1.44703237700413;7.54638051006576;-2.1448878321982]
Robot Arm 2 end: [7.70207854264995;-1.84566305313956;0.194995548107603]
Robot Arm 1 togoal: 19.5349707852231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993111e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-0.878386867877313;-8.82197962512671;18.8362868545846]
Robot Arm 1 end: [-8.46836506420622;19.3478881952024;0.269323219085295]
Robot Arm 1 togoal: 20.7371699279019
Mobile Platform 2 Pose: [1.45758676147664;7.53915747889068;-2.19953015258418]
Robot Arm 2 end: [7.68465644124394;-1.8627850531292;0.210158865503289]
Robot Arm 1 togoal: 19.5110134287355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-0.878386867720474;-8.8219796251839;19.1898402451153]
Robot Arm 1 end: [-8.4141022495187;19.6133172394145;0.211482189719231]
Robot Arm 1 togoal: 20.813694215511
Mobile Platform 2 Pose: [1.46844094075542;7.53172801158065;-2.25132722251624]
Robot Arm 2 end: [7.67930625514135;-1.91362981308734;0.210923658879318]
Robot Arm 1 togoal: 19.484957081873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-0.891654812125403;-8.81321726595017;19.539413252251]
Robot Arm 1 end: [-8.39455767402407;19.9465756391695;0.203055157064307]
Robot Arm 1 togoal: 20.9530689298188
Mobile Platform 2 Pose: [1.47937482917279;7.52424403799854;-2.30279325148873]
Robot Arm 2 end: [7.67392241716527;-1.96415527429893;0.211702978948092]
Robot Arm 1 togoal: 19.4591005124309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.085262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-0.91292990838175;-8.79966880552056;19.8865841140022]
Robot Arm 1 end: [-8.38068954272167;20.2920736720363;0.199332468385969]
Robot Arm 1 togoal: 21.1074236608986
Mobile Platform 2 Pose: [1.49038997728518;7.51670481352918;-2.35393479346844]
Robot Arm 2 end: [7.67323234167812;-2.03356666688202;0.203021304202165]
Robot Arm 1 togoal: 19.4304984315001

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.618321e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-0.933135205884429;-8.78673853395155;20.2340759152621]
Robot Arm 1 end: [-8.35991017119797;20.6246457505386;0.194213323362985]
Robot Arm 1 togoal: 21.2539352051378
Mobile Platform 2 Pose: [1.50143868078778;7.50915300225324;-2.40593678296965]
Robot Arm 2 end: [7.62371631076079;-2.068064783425;0.187408910297086]
Robot Arm 1 togoal: 19.3723885485671

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.736799e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-0.951923404682841;-8.77533736743947;20.5813443633787]
Robot Arm 1 end: [-8.35051938811416;20.9743368365487;0.193680554020229]
Robot Arm 1 togoal: 21.4228711364107
Mobile Platform 2 Pose: [1.51340161785289;7.50097807078148;-2.46166037695352]
Robot Arm 2 end: [7.57385347558896;-2.12694354493087;0.16429102332604]
Robot Arm 1 togoal: 19.3045591292203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.809148e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-0.95836216833398;-8.77095690397051;20.9312566631185]
Robot Arm 1 end: [-8.34851790206346;21.3250005458024;0.19181439243227]
Robot Arm 1 togoal: 21.6030201810796
Mobile Platform 2 Pose: [1.52613904397242;7.49227647235563;-2.52203117847609]
Robot Arm 2 end: [7.52721230643165;-2.21172381990035;0.135207497614265]
Robot Arm 1 togoal: 19.229686371647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.860935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-0.957979182482557;-8.77076868760539;21.2822318967492]
Robot Arm 1 end: [-8.35047155072176;21.6714808323639;0.187321801977158]
Robot Arm 1 togoal: 21.7885742647372
Mobile Platform 2 Pose: [1.53947012060765;7.48317092775811;-2.586749299578]
Robot Arm 2 end: [7.49613482664686;-2.32694722008758;0.103134751732612]
Robot Arm 1 togoal: 19.157265324095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.892103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-0.949511624560564;-8.77563304564856;21.6332450197098]
Robot Arm 1 end: [-8.35232508778565;22.0107011353;0.182881020060906]
Robot Arm 1 togoal: 21.9739696070996
Mobile Platform 2 Pose: [1.55307311632894;7.4738816172091;-2.65420326967983]
Robot Arm 2 end: [7.47993712557376;-2.46185999726224;0.0753630674931314]
Robot Arm 1 togoal: 19.0908779985499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.852452e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-0.944313212901028;-8.77696856061278;21.9811901615613]
Robot Arm 1 end: [-8.36661429286925;22.3726435931717;0.181926611708023]
Robot Arm 1 togoal: 22.1857600186677
Mobile Platform 2 Pose: [1.56665689315148;7.46460347947513;-2.72287415976673]
Robot Arm 2 end: [7.37222627410331;-2.51700068839425;0.0676044351985747]
Robot Arm 1 togoal: 18.9710801770464

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977300e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-0.94327307042688;-8.77780067461984;22.3344315092113]
Robot Arm 1 end: [-8.36527317282988;22.7123158391124;0.176742217031264]
Robot Arm 1 togoal: 22.3761762080362
Mobile Platform 2 Pose: [1.58185418815914;7.45426492570305;-2.80551276775301]
Robot Arm 2 end: [7.25293225221968;-2.65894463916553;0.0872795658824292]
Robot Arm 1 togoal: 18.8045292255277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974320e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-0.938665311391074;-8.78071461255372;22.686602572058]
Robot Arm 1 end: [-8.37962790971212;23.0655047006127;0.169032786515717]
Robot Arm 1 togoal: 22.590837756213
Mobile Platform 2 Pose: [1.59793374973538;7.44331189797532;-2.89131250979025]
Robot Arm 2 end: [7.19694398460017;-2.83794901035123;0.0978354888217626]
Robot Arm 1 togoal: 18.6830204735124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-0.932848149788564;-8.78410308623561;23.0384108139759]
Robot Arm 1 end: [-8.40332102184144;23.4442203293692;0.171369133720863]
Robot Arm 1 togoal: 22.8309173791328
Mobile Platform 2 Pose: [1.61411881267825;7.43226814377946;-2.97186790706792]
Robot Arm 2 end: [7.22138146203779;-3.00817508122283;0.086530201594932]
Robot Arm 1 togoal: 18.6418695081365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967622e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-0.932848149569898;-8.78410308624796;23.3919642045607]
Robot Arm 1 end: [-8.4267902475091;23.8332732940303;0.179549807724076]
Robot Arm 1 togoal: 23.0804145862111
Mobile Platform 2 Pose: [1.62979435893193;7.42161215347986;-3.04701440922675]
Robot Arm 2 end: [7.14973824231948;-3.15897474989502;0.143112313588963]
Robot Arm 1 togoal: 18.5151754711915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-0.932848150275363;-8.78410308593356;23.7455175953506]
Robot Arm 1 end: [-8.45476568483841;24.221933303066;0.18751170672485]
Robot Arm 1 togoal: 23.3370939576343
Mobile Platform 2 Pose: [1.64569536551878;7.41078071721717;-3.11430726279201]
Robot Arm 2 end: [7.15184540669363;-3.31491662298713;0.17122138372735]
Robot Arm 1 togoal: 18.4580075556837

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992782e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-0.932848149951756;-8.78410308601194;24.0990709857844]
Robot Arm 1 end: [-8.49263475145225;24.5695718156016;0.159552310888551]
Robot Arm 1 togoal: 23.5818468901564
Mobile Platform 2 Pose: [1.66116754668625;7.40026373744526;-3.17109020691264]
Robot Arm 2 end: [7.1913451033443;-3.4421689720055;0.198841645884118]
Robot Arm 1 togoal: 18.447102671351

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991552e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-0.932848150710025;-8.78410308563252;24.4526243765919]
Robot Arm 1 end: [-8.53211165842568;24.9190107731482;0.135609783093938]
Robot Arm 1 togoal: 23.8313942316174
Mobile Platform 2 Pose: [1.67537211958791;7.39059270936304;-3.21456891220976]
Robot Arm 2 end: [7.23717879766593;-3.53733659481556;0.217090787582641]
Robot Arm 1 togoal: 18.4549866005273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987911e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969104e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-0.932848150815642;-8.78410308566545;24.806177767315]
Robot Arm 1 end: [-8.56918123426533;25.3407779294431;0.173166293954435]
Robot Arm 1 togoal: 24.1240671299562
Mobile Platform 2 Pose: [1.68838755221946;7.38172840211975;-3.24865785189116]
Robot Arm 2 end: [7.29211221228514;-3.60773052381502;0.231101129564109]
Robot Arm 1 togoal: 18.4808574723788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991446e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-0.932848150328486;-8.78410308594714;25.1597311580987]
Robot Arm 1 end: [-8.61811421002679;25.7300939566201;0.187507401269621]
Robot Arm 1 togoal: 24.4100027679754
Mobile Platform 2 Pose: [1.70028730202405;7.37365303481856;-3.27650558520712]
Robot Arm 2 end: [7.34840196440673;-3.6582692186852;0.23682780924237]
Robot Arm 1 togoal: 18.5157689075237

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993286e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955162e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-0.931113781312784;-8.78511200699026;25.5127642381638]
Robot Arm 1 end: [-8.67365285262141;26.1028286870136;0.191999359502877]
Robot Arm 1 togoal: 24.6935749798599
Mobile Platform 2 Pose: [1.71099906966291;7.36638487893605;-3.3016196858917]
Robot Arm 2 end: [7.41702670113234;-3.64520301885094;0.22525899168243]
Robot Arm 1 togoal: 18.5853492983106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987058e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-0.883336875090277;-8.80301997374736;25.8212283713617]
Robot Arm 1 end: [-8.79238340106021;26.438900731962;0.206244186413155]
Robot Arm 1 togoal: 25.0023941657015
Mobile Platform 2 Pose: [1.71977573301021;7.36042650382435;-3.33034638414497]
Robot Arm 2 end: [7.46893516710575;-3.62661933988653;0.216491932646751]
Robot Arm 1 togoal: 18.6409607641088

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993050e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-0.82630754128446;-8.82761567903817;26.0828144980256]
Robot Arm 1 end: [-8.88856094738362;26.7121998261954;0.220595253732366]
Robot Arm 1 togoal: 25.2540375028989
Mobile Platform 2 Pose: [1.72842156217933;7.35483403557263;-3.37025376265831]
Robot Arm 2 end: [7.51492889531095;-3.64085648632534;0.215071660819811]
Robot Arm 1 togoal: 18.6793171519299

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-0.77197256385144;-8.85490162711577;26.3311670221078]
Robot Arm 1 end: [-8.97086695015681;26.9723652545574;0.241605167916587]
Robot Arm 1 togoal: 25.4870913662391
Mobile Platform 2 Pose: [1.73593861048865;7.35001948373875;-3.42154837190479]
Robot Arm 2 end: [7.55607993716473;-3.65773061539529;0.213732740693446]
Robot Arm 1 togoal: 18.7122759544174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-0.706015869762931;-8.88853206618962;26.5477528066974]
Robot Arm 1 end: [-9.05991243495162;27.2143335505489;0.289868434994087]
Robot Arm 1 togoal: 25.7124333066307
Mobile Platform 2 Pose: [1.74243880073626;7.34590678969563;-3.48720413551595]
Robot Arm 2 end: [7.59089420351383;-3.68139338870819;0.212487776926366]
Robot Arm 1 togoal: 18.7370369222407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-0.586582762024953;-8.94961804803507;26.744020025052]
Robot Arm 1 end: [-9.1363278381974;27.4498083727946;0.36324398981758]
Robot Arm 1 togoal: 25.923642767893
Mobile Platform 2 Pose: [1.74747893068429;7.34269091312919;-3.56977182015761]
Robot Arm 2 end: [7.6075819482982;-3.72498316713218;0.208836267938039]
Robot Arm 1 togoal: 18.7383177467602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-0.468819436083146;-9.01029380202366;26.9571016357413]
Robot Arm 1 end: [-9.21192292670059;27.7035845321611;0.467539535436253]
Robot Arm 1 togoal: 26.1462170644102
Mobile Platform 2 Pose: [1.75255142057957;7.33955762198693;-3.66456173777278]
Robot Arm 2 end: [7.63097931628077;-3.77444898836954;0.210270832042819]
Robot Arm 1 togoal: 18.7437170273472

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-0.354037998124777;-9.06979461152879;27.1854648552489]
Robot Arm 1 end: [-9.28151055011921;27.9618990630148;0.600122801662075]
Robot Arm 1 togoal: 26.3676787347448
Mobile Platform 2 Pose: [1.75650451216142;7.33709212272719;-3.78055791429252]
Robot Arm 2 end: [7.63705505662851;-3.8490235993157;0.207595672611307]
Robot Arm 1 togoal: 18.7249900581956

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-0.245168996259149;-9.12633484670027;27.42064955961]
Robot Arm 1 end: [-9.33970038013284;28.2050299876336;0.739642916314781]
Robot Arm 1 togoal: 26.5717452670346
Mobile Platform 2 Pose: [1.76018163255461;7.33481463972201;-3.91106217889485]
Robot Arm 2 end: [7.64023080644596;-3.94036325476379;0.206391711117315]
Robot Arm 1 togoal: 18.6982655704239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-0.146272131636683;-9.17748207878954;27.6501495588034]
Robot Arm 1 end: [-9.38597334180162;28.4235618479494;0.854154510851294]
Robot Arm 1 togoal: 26.7523508589555
Mobile Platform 2 Pose: [1.76428231905248;7.33235403501312;-4.05713328785145]
Robot Arm 2 end: [7.64234871844416;-4.03335494850378;0.207622367557531]
Robot Arm 1 togoal: 18.6702512644885

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989710e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-0.0846564238607329;-9.20939714435323;27.8662950923273]
Robot Arm 1 end: [-9.41175954058374;28.624062010732;0.927932954544542]
Robot Arm 1 togoal: 26.9076907734841
Mobile Platform 2 Pose: [1.76657967263224;7.3307962337642;-4.22565711355214]
Robot Arm 2 end: [7.67023705600563;-4.17309803506241;0.218170090869017]
Robot Arm 1 togoal: 18.6517406666658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-0.0510523521861334;-9.2268174156991;28.0696944738842]
Robot Arm 1 end: [-9.42507633354414;28.8160114924378;0.967950471100904]
Robot Arm 1 togoal: 27.0496163157444
Mobile Platform 2 Pose: [1.76871774023431;7.32935118206539;-4.39707259924825]
Robot Arm 2 end: [7.66838571611014;-4.30588710257553;0.221686524996867]
Robot Arm 1 togoal: 18.6086805284035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-0.0491814779987497;-9.22807230615299;28.2643210241784]
Robot Arm 1 end: [-9.42420750144924;29.00426846223;0.987642170592277]
Robot Arm 1 togoal: 27.1798919684766
Mobile Platform 2 Pose: [1.77090315631644;7.32785570757782;-4.58053699355755]
Robot Arm 2 end: [7.6711961247224;-4.48906567013416;0.22667032429192]
Robot Arm 1 togoal: 18.5557769704186

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-0.0481712824519081;-9.22874001915875;28.4535636567583]
Robot Arm 1 end: [-9.42423795405052;29.190384983581;0.996796283300593]
Robot Arm 1 togoal: 27.3101938010376
Mobile Platform 2 Pose: [1.77304233794508;7.32639255461359;-4.76516604912336]
Robot Arm 2 end: [7.66984100824413;-4.67263297345191;0.228284281353363]
Robot Arm 1 togoal: 18.5006820042239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-0.0475815779071041;-9.22912094325936;28.6393953631059]
Robot Arm 1 end: [-9.42476100648005;29.3748169108023;1.00084605936771]
Robot Arm 1 togoal: 27.4403977851622
Mobile Platform 2 Pose: [1.77516920762137;7.32493873003858;-4.95270403637849]
Robot Arm 2 end: [7.66572631199122;-4.85570905443348;0.231430872954742]
Robot Arm 1 togoal: 18.4446361849792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-0.0672773742654112;-9.21932294122607;28.8228438907367]
Robot Arm 1 end: [-9.41522481019492;29.5577791567436;1.00249490134347]
Robot Arm 1 togoal: 27.56313711303
Mobile Platform 2 Pose: [1.77734239728866;7.32345339978655;-5.14438608228356]
Robot Arm 2 end: [7.66151537687052;-5.04058875983936;0.238706490748119]
Robot Arm 1 togoal: 18.389376332555

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-0.15822744732288;-9.17388072563331;29.0044653766113]
Robot Arm 1 end: [-9.36930845228721;29.7395609233397;1.00301524805349]
Robot Arm 1 togoal: 27.6602365060267
Mobile Platform 2 Pose: [1.77962027509186;7.32189872474204;-5.34018989391103]
Robot Arm 2 end: [7.6519356532724;-5.2292646667218;0.240636102138746]
Robot Arm 1 togoal: 18.3300321777788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-0.286942628465326;-9.10954732907753;29.1846777391906]
Robot Arm 1 end: [-9.30410323874082;29.9202664098585;1.00296499108036]
Robot Arm 1 togoal: 27.744076412131
Mobile Platform 2 Pose: [1.78205392232491;7.32023884232855;-5.54238515562467]
Robot Arm 2 end: [7.64036406758696;-5.42270428736533;0.245061865301015]
Robot Arm 1 togoal: 18.2691570304023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-0.420283098198722;-9.04289681543193;29.3637022024679]
Robot Arm 1 end: [-9.23656134519837;30.0999022987735;1.00262454868295]
Robot Arm 1 togoal: 27.8266073734537
Mobile Platform 2 Pose: [1.78473662395216;7.31841272556877;-5.75151441015716]
Robot Arm 2 end: [7.61776331422134;-5.61642159650657;0.253563205409617]
Robot Arm 1 togoal: 18.199113710392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-0.552224406829753;-8.97694373859375;29.5416227670292]
Robot Arm 1 end: [-9.1697489211479;30.2784770870026;1.00214594774597]
Robot Arm 1 togoal: 27.9099465459087
Mobile Platform 2 Pose: [1.78797657873633;7.31621019013728;-5.97111060058879]
Robot Arm 2 end: [7.59141542710026;-5.82308845340157;0.25757925155392]
Robot Arm 1 togoal: 18.1245887893914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-0.664035826461458;-8.92105456873228;29.7184766222927]
Robot Arm 1 end: [-9.11326100412082;30.455938394044;1.00160319172347]
Robot Arm 1 togoal: 28.0005702591397
Mobile Platform 2 Pose: [1.80321361575264;7.30789481824541;-6.20221058883043]
Robot Arm 2 end: [7.55742381533746;-6.04426900702579;0.258795622396531]
Robot Arm 1 togoal: 18.0417321399858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010086e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-0.765142128578298;-8.87051764895362;29.8942418142514]
Robot Arm 1 end: [-9.06225701644592;30.6322794538702;1.00104325023465]
Robot Arm 1 togoal: 28.0950343129397
Mobile Platform 2 Pose: [1.85982297169526;7.27876582242756;-6.44503412030905]
Robot Arm 2 end: [7.43760963884187;-6.27808579562659;0.262043310123911]
Robot Arm 1 togoal: 17.8747824380699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012749e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-0.864371018718857;-8.82091930852225;30.0689083135266]
Robot Arm 1 end: [-9.01220721476315;30.8075145699354;1.00048109314248]
Robot Arm 1 togoal: 28.1902044279137
Mobile Platform 2 Pose: [1.89568903974315;7.26086294047299;-6.6956660099142]
Robot Arm 2 end: [7.36731119999555;-6.53392081339579;0.265986386949378]
Robot Arm 1 togoal: 17.7542181583489

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012604e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-0.957665476338686;-8.77428811101789;30.2424836384603]
Robot Arm 1 end: [-8.96519248295595;30.9816375119019;0.999917448631095]
Robot Arm 1 togoal: 28.2874383008034
Mobile Platform 2 Pose: [1.94560467306415;7.23624598886313;-6.95215422039159]
Robot Arm 2 end: [7.30553510659911;-6.82734983072575;0.269119696122852]
Robot Arm 1 togoal: 17.6384369692319

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010932e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-0.991013527149897;-8.75763000843242;30.4149655792342]
Robot Arm 1 end: [-8.94891559638599;31.1544650168927;0.999355585356769]
Robot Arm 1 togoal: 28.4050154959512
Mobile Platform 2 Pose: [2.0088458895246;7.20614222268831;-7.20995564259383]
Robot Arm 2 end: [7.2469333121188;-7.14055133823643;0.269759428465879]
Robot Arm 1 togoal: 17.5270864175934

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006366e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-0.990813291890365;-8.75774659431449;30.5862299336704]
Robot Arm 1 end: [-8.94983102046717;31.3259460647535;0.998835509326198]
Robot Arm 1 togoal: 28.5335719288062
Mobile Platform 2 Pose: [2.04399275635341;7.18736357892485;-7.47010786465452]
Robot Arm 2 end: [7.20182057930118;-7.44405889254278;0.257382696490192]
Robot Arm 1 togoal: 17.4365190011363

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002749e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-0.990611027994942;-8.75786471282491;30.7562044499296]
Robot Arm 1 end: [-8.95073245955493;31.4961201721111;0.998357305044557]
Robot Arm 1 togoal: 28.6615875100117
Mobile Platform 2 Pose: [2.05381655101845;7.1809684372759;-7.74203073398954]
Robot Arm 2 end: [7.21042949273291;-7.73939988121998;0.260960497843583]
Robot Arm 1 togoal: 17.4039333418729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001669e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-0.99040866649699;-8.7579830450481;30.9248931629071]
Robot Arm 1 end: [-8.95162268458987;31.6649998725757;0.997897389069139]
Robot Arm 1 togoal: 28.7890579595674
Mobile Platform 2 Pose: [2.05919771320321;7.17684190994498;-8.02813884500538]
Robot Arm 2 end: [7.19488644145229;-8.07539890431767;0.251158840102287]
Robot Arm 1 togoal: 17.3487955110236

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-0.990207067621224;-8.75810100277619;31.0923057997301]
Robot Arm 1 end: [-8.95250300571908;31.8325987085239;0.997445195516501]
Robot Arm 1 togoal: 28.915979080476
Mobile Platform 2 Pose: [2.0546605890926;7.17752534744501;-8.31223538749233]
Robot Arm 2 end: [7.17762966955566;-8.43006281072624;0.242090878003618]
Robot Arm 1 togoal: 17.2965682256623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968645e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-0.977501611301659;-8.76447081667368;31.2584546378214]
Robot Arm 1 end: [-8.9597881639137;31.998888053666;0.996996034350754]
Robot Arm 1 togoal: 29.0465010452933
Mobile Platform 2 Pose: [2.04966632209373;7.18049885806545;-8.60071657491195]
Robot Arm 2 end: [7.15993182831643;-8.77480454424611;0.2367059432804]
Robot Arm 1 togoal: 17.2514861818161

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-0.927077823262634;-8.78969987626053;31.4233254505952]
Robot Arm 1 end: [-8.98641019672717;32.1637640736055;0.99655719473649]
Robot Arm 1 togoal: 29.1888891588019
Mobile Platform 2 Pose: [2.03311042625893;7.18747579242205;-8.87692812723858]
Robot Arm 2 end: [7.19071326140434;-9.11089509320818;0.239106388307993]
Robot Arm 1 togoal: 17.2613551380765

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-0.867113714364268;-8.81969949679782;31.586846900889]
Robot Arm 1 end: [-9.01791200618432;32.3272424032434;0.996151339797283]
Robot Arm 1 togoal: 29.3336359240425
Mobile Platform 2 Pose: [2.0080612685043;7.19831806539187;-9.13322852535321]
Robot Arm 2 end: [7.1452236355147;-9.37036244927346;0.268620790528241]
Robot Arm 1 togoal: 17.202429788882

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-0.782242263794557;-8.86215288120062;31.7490048204984]
Robot Arm 1 end: [-9.06217706859282;32.4892582467116;0.995768210753863]
Robot Arm 1 togoal: 29.4857457004924
Mobile Platform 2 Pose: [2.0784887496501;7.16709255566641;-9.39364115797823]
Robot Arm 2 end: [7.07394025345754;-9.62486607259393;0.306812759172217]
Robot Arm 1 togoal: 17.1211685319057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000820e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-0.696029379086457;-8.90527743472448;31.9097546256361]
Robot Arm 1 end: [-9.10711663693061;32.6498452453891;0.99541538161843]
Robot Arm 1 togoal: 29.6373688948331
Mobile Platform 2 Pose: [2.05383018210642;7.17905143515994;-9.6792949414988]
Robot Arm 2 end: [7.17954941688319;-10.0143265908413;0.310011130590682]
Robot Arm 1 togoal: 17.2221838760411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990954e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-0.61234131014075;-8.94713960803012;32.0691013075644]
Robot Arm 1 end: [-9.15074979126803;32.8090271164606;0.995078614001033]
Robot Arm 1 togoal: 29.7872376844755
Mobile Platform 2 Pose: [2.07680471199683;7.16774729388023;-9.92747074256162]
Robot Arm 2 end: [7.15667727096701;-10.2666585616846;0.324554448773388]
Robot Arm 1 togoal: 17.2004117312838

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-0.522913260184529;-8.99187175080947;32.2270621736596]
Robot Arm 1 end: [-9.1973174003387;32.9667951487974;0.994748511022697]
Robot Arm 1 togoal: 29.9380629868704
Mobile Platform 2 Pose: [2.09387394584264;7.15943578060863;-10.1773839372371]
Robot Arm 2 end: [7.1729296167247;-10.512296916828;0.318325361455379]
Robot Arm 1 togoal: 17.2226126368263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-0.428362799995666;-9.03916514886203;32.3836385481304]
Robot Arm 1 end: [-9.24650230651515;33.1231516271453;0.994427435322673]
Robot Arm 1 togoal: 30.0896206501929
Mobile Platform 2 Pose: [2.09370013657343;7.15983444177382;-10.4230911922306]
Robot Arm 2 end: [7.2104525776243;-10.7536747876714;0.310625401723865]
Robot Arm 1 togoal: 17.2694154585608

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-0.329184473026738;-9.08877245926724;32.5388325464567]
Robot Arm 1 end: [-9.2980505957198;33.2780997947868;0.99411618097319]
Robot Arm 1 togoal: 30.2417309922856
Mobile Platform 2 Pose: [2.09302947554727;7.16059268565104;-10.6604301838298]
Robot Arm 2 end: [7.26169523602795;-10.9847848664288;0.303203390792727]
Robot Arm 1 togoal: 17.3325979655692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-0.240913356521784;-9.13292620397429;32.6926476926245]
Robot Arm 1 end: [-9.34399433845789;33.4317000126698;0.993814906888994]
Robot Arm 1 togoal: 30.3893502163577
Mobile Platform 2 Pose: [2.09185253932578;7.16172278901315;-10.8844948512087]
Robot Arm 2 end: [7.33040976737652;-11.196423039621;0.298113321032559]
Robot Arm 1 togoal: 17.4146485070619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971684e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-0.148287712385014;-9.17925705630968;32.8451196546809]
Robot Arm 1 end: [-9.39216368892995;33.5839367288766;0.993510935298382]
Robot Arm 1 togoal: 30.5374577999428
Mobile Platform 2 Pose: [2.09027544062263;7.1631556344789;-11.0891092468567]
Robot Arm 2 end: [7.41426839193962;-11.3757616125562;0.299373737204199]
Robot Arm 1 togoal: 17.5111916889469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960976e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-0.051631706163255;-9.22760305467085;32.9962550249931]
Robot Arm 1 end: [-9.44239122334963;33.7348150675221;0.99321106880527]
Robot Arm 1 togoal: 30.6859346269125
Mobile Platform 2 Pose: [2.08838109096492;7.164791422766;-11.2695974397735]
Robot Arm 2 end: [7.49566134781182;-11.5191817925591;0.309912261991361]
Robot Arm 1 togoal: 17.6032046180607

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.932725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [0.0487707872741803;-9.27782225605746;33.1460594179545]
Robot Arm 1 end: [-9.49453075327538;33.8843403824268;0.992918298595468]
Robot Arm 1 togoal: 30.834677152151
Mobile Platform 2 Pose: [2.08780951427004;7.16573544186978;-11.4271491756174]
Robot Arm 2 end: [7.57563701618563;-11.6370601063475;0.327426312997718]
Robot Arm 1 togoal: 17.6920208134292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945161e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [0.152661657390074;-9.32978566491414;33.2945378156676]
Robot Arm 1 end: [-9.54844968630832;34.0325179221116;0.992633902372669]
Robot Arm 1 togoal: 30.9835920052512
Mobile Platform 2 Pose: [2.08700188905093;7.16674026788697;-11.5696446332035]
Robot Arm 2 end: [7.63407193385338;-11.7518767200562;0.352836315913019]
Robot Arm 1 togoal: 17.7593403988833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964058e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [0.235160766927876;-9.37105318698934;33.4416952598684]
Robot Arm 1 end: [-9.59138745849615;34.1794480448731;0.992358384018384]
Robot Arm 1 togoal: 31.1247109586866
Mobile Platform 2 Pose: [2.0864929721311;7.16756144166463;-11.7019122412862]
Robot Arm 2 end: [7.68718707870793;-11.8524679592218;0.38424435763092]
Robot Arm 1 togoal: 17.8202255229168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000229e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [0.248891520390741;-9.37793621385227;33.5875887361716]
Robot Arm 1 end: [-9.59905021126067;34.3253800932588;0.992070116632214]
Robot Arm 1 togoal: 31.2430256647231
Mobile Platform 2 Pose: [2.08629719572851;7.16818166879021;-11.8259054598124]
Robot Arm 2 end: [7.73252628696012;-11.9427406915057;0.419258569575102]
Robot Arm 1 togoal: 17.872620515517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000396e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [0.249076161204022;-9.37804511793427;33.7323799700362]
Robot Arm 1 end: [-9.59977055560083;34.4702830934055;0.991716268344671]
Robot Arm 1 togoal: 31.3564331800063
Mobile Platform 2 Pose: [2.08730662548041;7.16815442416142;-11.9439330894934]
Robot Arm 2 end: [7.77476371622013;-12.013731794164;0.455664617205453]
Robot Arm 1 togoal: 17.9201640724664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [0.24925618715101;-9.37815099366383;33.8761287413014]
Robot Arm 1 end: [-9.6004896019516;34.6141565934659;0.991320558558023]
Robot Arm 1 togoal: 31.46929388713
Mobile Platform 2 Pose: [2.02827990931932;7.1980990676928;-12.0616881366291]
Robot Arm 2 end: [7.87728098517445;-12.0650572256728;0.632113367132478]
Robot Arm 1 togoal: 18.0180952437896

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [0.139642165691353;-9.32335956343086;34.018859489684]
Robot Arm 1 end: [-9.54489384393944;34.757443091139;0.990905907699638]
Robot Arm 1 togoal: 31.5470262232202
Mobile Platform 2 Pose: [1.97217042096639;7.22641167587357;-12.1667358245257]
Robot Arm 2 end: [7.90536369721156;-12.1073832303331;0.677122790541518]
Robot Arm 1 togoal: 18.0486908653662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-0.00676261813010395;-9.25017094552569;34.1608167661818]
Robot Arm 1 end: [-9.4704410763705;34.9001327127649;0.990382289706816]
Robot Arm 1 togoal: 31.6132288761556
Mobile Platform 2 Pose: [1.92485311868113;7.25039500852109;-12.2949868956802]
Robot Arm 2 end: [7.91252161753018;-12.1798477781221;0.680576616549232]
Robot Arm 1 togoal: 18.064232020701

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [0.120825834269432;-9.31397762394201;34.3021187830254]
Robot Arm 1 end: [-9.53652980628908;35.0411367468179;0.989773479113454]
Robot Arm 1 togoal: 31.7650236590711
Mobile Platform 2 Pose: [1.92264371511498;7.25190578725929;-12.4525957710568]
Robot Arm 2 end: [7.89893204598582;-12.2914051145991;0.714229780768351]
Robot Arm 1 togoal: 18.0630336314991

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [0.244400228053522;-9.37578023078271;34.442235204709]
Robot Arm 1 end: [-9.60051287198514;35.1808914661945;0.989378433191938]
Robot Arm 1 togoal: 31.9145738083312
Mobile Platform 2 Pose: [1.88960393779318;7.26884110755829;-12.6306669695009]
Robot Arm 2 end: [7.90102612682232;-12.4301122674844;0.775969024868578]
Robot Arm 1 togoal: 18.0805766827184

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [0.134005872803182;-9.32059991478795;34.5811236542542]
Robot Arm 1 end: [-9.54446923664935;35.320298954543;0.989081071717932]
Robot Arm 1 togoal: 31.9904166061655
Mobile Platform 2 Pose: [1.86143285249233;7.28332870853609;-12.8239905956738]
Robot Arm 2 end: [7.88915052176525;-12.5842216722037;0.800170585827734]
Robot Arm 1 togoal: 18.0892095760372

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [0.169391142725829;-9.33830677522733;34.7192469683783]
Robot Arm 1 end: [-9.56322272525964;35.4584280210433;0.988609337950739]
Robot Arm 1 togoal: 32.1112858745612
Mobile Platform 2 Pose: [1.84039176704614;7.29420454789936;-13.0395393521763]
Robot Arm 2 end: [7.86554592478856;-12.7628236838195;0.793307633916337]
Robot Arm 1 togoal: 18.092552668937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [0.169561627766763;-9.33840694349496;34.8563708138157]
Robot Arm 1 end: [-9.56389764054663;35.595675034775;0.988194961344951]
Robot Arm 1 togoal: 32.220623094298
Mobile Platform 2 Pose: [1.82748266740578;7.30089351736993;-13.280403344672]
Robot Arm 2 end: [7.83300236365696;-12.9733952230042;0.772602066827055]
Robot Arm 1 togoal: 18.0946719207825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [0.169730455168947;-9.3385060936645;34.9925604910492]
Robot Arm 1 end: [-9.56456701661627;35.7319888536223;0.987774243322833]
Robot Arm 1 togoal: 32.3294271928563
Mobile Platform 2 Pose: [1.82536890989345;7.30146655179661;-13.5306302718917]
Robot Arm 2 end: [7.79101570490192;-13.2016827568377;0.743504148193044]
Robot Arm 1 togoal: 18.0935212738448

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [0.169897946117853;-9.33860444393683;35.1278289722374]
Robot Arm 1 end: [-9.56523038045312;35.8673811756587;0.987351035659207]
Robot Arm 1 togoal: 32.4377007499633
Mobile Platform 2 Pose: [1.82830268409769;7.2997050662499;-13.8075467896478]
Robot Arm 2 end: [7.75553225424301;-13.4673297183381;0.71733840845064]
Robot Arm 1 togoal: 18.1087595643895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [0.170064254381904;-9.33870209828763;35.2621879089276]
Robot Arm 1 end: [-9.56588754935725;36.0018630691709;0.98692713748541]
Robot Arm 1 togoal: 32.5454462634096
Mobile Platform 2 Pose: [1.83710794132479;7.29453471956204;-14.0979827148695]
Robot Arm 2 end: [7.7126480531054;-13.7546544554605;0.676873992471368]
Robot Arm 1 togoal: 18.1258897671169

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [0.110379887504676;-9.30887439780978;35.3956483094906]
Robot Arm 1 end: [-9.53584087017043;36.1356827659486;0.986503394402865]
Robot Arm 1 togoal: 32.6344707137762
Mobile Platform 2 Pose: [1.85931910849438;7.28222675711001;-14.3978953505801]
Robot Arm 2 end: [7.63422021647742;-14.0587923501308;0.604751933418207]
Robot Arm 1 togoal: 18.1184720245449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-0.0120019063599806;-9.24769694750195;35.5283408606297]
Robot Arm 1 end: [-9.47364062974386;36.2690010330973;0.986020437534558]
Robot Arm 1 togoal: 32.704270329508
Mobile Platform 2 Pose: [1.88220524150317;7.26985005377528;-14.7014481638086]
Robot Arm 2 end: [7.55460541117763;-14.3758927674106;0.545987199132928]
Robot Arm 1 togoal: 18.1180368311768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990266e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-0.169251973550064;-9.16908382991979;35.6604197179951]
Robot Arm 1 end: [-9.39356599180758;36.4018700895201;0.985446388920231]
Robot Arm 1 togoal: 32.7636378287399
Mobile Platform 2 Pose: [1.92293212959242;7.2495465410979;-14.9968418975749]
Robot Arm 2 end: [7.50488218702771;-14.6819535583534;0.469000359530587]
Robot Arm 1 togoal: 18.15071053011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988256e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-0.320139749042539;-9.0936507453311;35.7919911984693]
Robot Arm 1 end: [-9.3167631450942;36.5342290703333;0.984793347963083]
Robot Arm 1 togoal: 32.8251453665369
Mobile Platform 2 Pose: [1.96925753568052;7.22734929609223;-15.2932725290371]
Robot Arm 2 end: [7.42730289987845;-15.0040783601251;0.378919562953327]
Robot Arm 1 togoal: 18.1674443531023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984863e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-0.458769842770316;-9.02434593533673;35.9230762651594]
Robot Arm 1 end: [-9.24624742661809;36.6660609637868;0.984108357437397]
Robot Arm 1 togoal: 32.8905016679109
Mobile Platform 2 Pose: [2.00897373517833;7.2075430570558;-15.5871447030817]
Robot Arm 2 end: [7.32713768029509;-15.335073708363;0.303940260627671]
Robot Arm 1 togoal: 18.170689201975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000352e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-0.584781066241613;-8.96135050133569;36.0536684017445]
Robot Arm 1 end: [-9.18219933077238;36.7973493248143;0.983419605798023]
Robot Arm 1 togoal: 32.9597283769427
Mobile Platform 2 Pose: [2.04791006154143;7.18823970623292;-15.8675714547096]
Robot Arm 2 end: [7.23423197388303;-15.6712865887102;0.252510311292146]
Robot Arm 1 togoal: 18.1876691577358

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002678e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [-0.699877442783074;-8.9038125062531;36.183751408775]
Robot Arm 1 end: [-9.12374241443968;36.9280806667439;0.982740874588609]
Robot Arm 1 togoal: 33.0322384227075
Mobile Platform 2 Pose: [2.0812541522455;7.17150555639736;-16.1292405793531]
Robot Arm 2 end: [7.18608794385281;-16.0050309221147;0.220800550481611]
Robot Arm 1 togoal: 18.2513707375069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968717e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [-0.773193568958628;-8.86716486015248;36.3133081302473]
Robot Arm 1 end: [-9.08671020975524;37.0581186116065;0.982077069630093]
Robot Arm 1 togoal: 33.1169854811065
Mobile Platform 2 Pose: [2.08794637669814;7.16747919963087;-16.3768178205221]
Robot Arm 2 end: [7.23490521955551;-16.2132682266233;0.206643257613009]
Robot Arm 1 togoal: 18.3677487360124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961100e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [-0.808611879572586;-8.84946670039019;36.4422591617835]
Robot Arm 1 end: [-9.06910517231898;37.1873882574153;0.981462845598326]
Robot Arm 1 togoal: 33.2125872625633
Mobile Platform 2 Pose: [2.09589786718297;7.16267358209843;-16.647017532395]
Robot Arm 2 end: [7.30262424150013;-16.4441667105566;0.200074566210828]
Robot Arm 1 togoal: 18.5108869111565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [-0.808465197013687;-8.84955187736842;36.5705223741871]
Robot Arm 1 end: [-9.06972855135006;37.3158121767471;0.980911265494687]
Robot Arm 1 togoal: 33.3181607796505
Mobile Platform 2 Pose: [2.10572923592853;7.1567846766675;-16.9390728298201]
Robot Arm 2 end: [7.38040460725737;-16.7017900057379;0.208265028146243]
Robot Arm 1 togoal: 18.673931741851

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.944757e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [-0.808314138377658;-8.8496401440612;36.6980161270529]
Robot Arm 1 end: [-9.07033659256327;37.4434435862046;0.980426877387611]
Robot Arm 1 togoal: 33.4232337293048
Mobile Platform 2 Pose: [2.11665334429885;7.15062821415567;-17.2371899027016]
Robot Arm 2 end: [7.50513719559661;-16.9585337902527;0.261148150351261]
Robot Arm 1 togoal: 18.8795722258162

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966124e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [-0.808161533490892;-8.84972956875118;36.8247273513214]
Robot Arm 1 end: [-9.07093439383507;37.5702809049712;0.979974768880632]
Robot Arm 1 togoal: 33.5278039264979
Mobile Platform 2 Pose: [2.1351236760367;7.14127500554237;-17.5149294585641]
Robot Arm 2 end: [7.60442693169754;-17.217794561243;0.340963439715308]
Robot Arm 1 togoal: 19.0631815866448

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996082e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [-0.808008713139177;-8.84981924253998;36.950654662751]
Robot Arm 1 end: [-9.07152440244751;37.6963281618067;0.979538471160055]
Robot Arm 1 togoal: 33.6318720452319
Mobile Platform 2 Pose: [2.14515205181032;7.13589156204163;-17.8257396754364]
Robot Arm 2 end: [7.66744579432505;-17.5310381333565;0.37048311200546]
Robot Arm 1 togoal: 19.2400328838348

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000203e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [-0.807131490606628;-8.85027114470159;37.0758021513306]
Robot Arm 1 end: [-9.07247965447818;37.821589196751;0.979110201661387]
Robot Arm 1 togoal: 33.7356480619062
Mobile Platform 2 Pose: [2.1536893333219;7.13146311320663;-18.1335942338864]
Robot Arm 2 end: [7.71389240747565;-17.8455878170283;0.395157221767228]
Robot Arm 1 togoal: 19.4062555476224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000072e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [-0.770468922700008;-8.86861562317346;37.2001750566058]
Robot Arm 1 end: [-9.09178753293365;37.9459337679087;0.978687069394433]
Robot Arm 1 togoal: 33.8491605313592
Mobile Platform 2 Pose: [2.16058873267102;7.12802846657994;-18.4372779511386]
Robot Arm 2 end: [7.74588852340791;-18.1557315397165;0.412805078186704]
Robot Arm 1 togoal: 19.5617129213645

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [-0.706945842047791;-8.90039096870682;37.3237116652058]
Robot Arm 1 end: [-9.12486224618075;38.0693214384443;0.978306381807166]
Robot Arm 1 togoal: 33.9697170773224
Mobile Platform 2 Pose: [2.16590798776532;7.12551264986636;-18.7371807676522]
Robot Arm 2 end: [7.76783361679512;-18.4587706377663;0.43067198474022]
Robot Arm 1 togoal: 19.7087677777509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [-0.635255302400519;-8.93625073964565;37.446356072169]
Robot Arm 1 end: [-9.16211190165239;38.191765558917;0.977976934287335]
Robot Arm 1 togoal: 34.0918912926561
Mobile Platform 2 Pose: [2.16865953334629;7.12481705150416;-19.0293101788396]
Robot Arm 2 end: [7.78738057995286;-18.7510352107218;0.453904999448899]
Robot Arm 1 togoal: 19.8522093706363

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999518e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-0.536401858974881;-8.98569258727833;37.5680847304774]
Robot Arm 1 end: [-9.21328428694663;38.3131718501972;0.977682731865191]
Robot Arm 1 togoal: 34.2210634984315
Mobile Platform 2 Pose: [2.17115324636577;7.12427274959572;-19.3191906184548]
Robot Arm 2 end: [7.80399816021649;-19.0412298154924;0.475352347048861]
Robot Arm 1 togoal: 19.9954886431831

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999436e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-0.437967989738334;-9.03492503616609;37.6888427718796]
Robot Arm 1 end: [-9.26422575894991;38.4335891224244;0.977437902436847]
Robot Arm 1 togoal: 34.3493063136441
Mobile Platform 2 Pose: [2.17318397928379;7.12400747273205;-19.6069578913447]
Robot Arm 2 end: [7.81769035477073;-19.330288238807;0.488819342207436]
Robot Arm 1 togoal: 20.1392592988926

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999402e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-0.334191245502067;-9.08682919288657;37.8086250209555]
Robot Arm 1 end: [-9.31789722710636;38.5529967616853;0.977218530588937]
Robot Arm 1 togoal: 34.4782609137458
Mobile Platform 2 Pose: [2.17482728263787;7.1239776942783;-19.892740279948]
Robot Arm 2 end: [7.8290517806954;-19.6180469661386;0.495762376378949]
Robot Arm 1 togoal: 20.283811945156

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999411e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [-0.235196940774768;-9.13634244845471;37.927422275734]
Robot Arm 1 end: [-9.36910545173709;38.6714278560169;0.977019794303747]
Robot Arm 1 togoal: 34.6050420570108
Mobile Platform 2 Pose: [2.17610299100151;7.12417270670057;-20.1765929265688]
Robot Arm 2 end: [7.83840122395664;-19.9042930432005;0.497499158285362]
Robot Arm 1 togoal: 20.4291735732368

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999451e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [-0.131428734984731;-9.18824250422474;38.045246960187]
Robot Arm 1 end: [-9.42275524289902;38.7888606390001;0.97682776339506]
Robot Arm 1 togoal: 34.7323774242481
Mobile Platform 2 Pose: [2.1770125986676;7.12459217818391;-20.458503100689]
Robot Arm 2 end: [7.84593682718731;-20.1888288394666;0.495055005419852]
Robot Arm 1 togoal: 20.5752826831567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [-0.0233048626988526;-9.24232048960782;38.1620976471869]
Robot Arm 1 end: [-9.47863127294111;38.9052936628284;0.976646786910934]
Robot Arm 1 togoal: 34.8601405393344
Mobile Platform 2 Pose: [2.17755107306019;7.1252392275369;-20.7383991311028]
Robot Arm 2 end: [7.85178771923052;-20.4714686987342;0.489170544146096]
Robot Arm 1 togoal: 20.722033044733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [0.0888291879139699;-9.29840379381333;38.2779724186141]
Robot Arm 1 end: [-9.53655596573277;39.020725563004;0.976478601854312]
Robot Arm 1 togoal: 34.9882270879569
Mobile Platform 2 Pose: [2.17770686690086;7.12612061855947;-21.0161523650855]
Robot Arm 2 end: [7.85604140399888;-20.7520179935558;0.480384803868262]
Robot Arm 1 togoal: 20.8692833020564

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999670e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [0.204678618575516;-9.35634473627047;38.3928691346692]
Robot Arm 1 end: [-9.59637758602558;39.1351549543004;0.976323758316767]
Robot Arm 1 togoal: 35.1165479640786
Mobile Platform 2 Pose: [2.17745913557063;7.12724858321478;-21.291573932035]
Robot Arm 2 end: [7.85876221110088;-21.0302561119818;0.469115685786323]
Robot Arm 1 togoal: 21.0168598562927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999943e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [0.322844075305138;-9.41544368964252;38.5067859516656]
Robot Arm 1 end: [-9.6573782673984;39.2485856092704;0.976182280773249]
Robot Arm 1 togoal: 35.2447032451344
Mobile Platform 2 Pose: [2.17658099849426;7.12884245776033;-21.5611782105982]
Robot Arm 2 end: [7.86611451335602;-21.3061852538636;0.464421473663175]
Robot Arm 1 togoal: 21.1694173510125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999956e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [0.46044525838543;-9.48426065051136;38.6197232855624]
Robot Arm 1 end: [-9.7283397628677;39.3609453916971;0.976052583071247]
Robot Arm 1 togoal: 35.3775277793768
Mobile Platform 2 Pose: [2.17544335975735;7.13059204808004;-21.829482647843]
Robot Arm 2 end: [7.87013765303201;-21.5806675653542;0.451374525633505]
Robot Arm 1 togoal: 21.3212987327924

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000010e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [0.593368916671476;-9.55073927182169;38.731650969036]
Robot Arm 1 end: [-9.79688883467996;39.4722994088396;0.97595489835348]
Robot Arm 1 togoal: 35.5081724978883
Mobile Platform 2 Pose: [2.17386871846727;7.13267864686413;-22.0929698690244]
Robot Arm 2 end: [7.87613353117123;-21.8528826558648;0.439205110152702]
Robot Arm 1 togoal: 21.4759693020284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000004e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [0.736731961326635;-9.62243764122339;38.8425761595979]
Robot Arm 1 end: [-9.87078320335548;39.5825959472507;0.975869941996504]
Robot Arm 1 togoal: 35.640919949872
Mobile Platform 2 Pose: [2.17207151024574;7.13490619482598;-22.3559470214936]
Robot Arm 2 end: [7.87457012788884;-22.1217551143866;0.413287257873363]
Robot Arm 1 togoal: 21.6255458157487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000018e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [0.887389036151376;-9.69778340646372;38.9524850113764]
Robot Arm 1 end: [-9.94840743306089;39.6918309583941;0.975806928545425]
Robot Arm 1 togoal: 35.7748657616267
Mobile Platform 2 Pose: [2.17005082994063;7.13729637447932;-22.6159304746315]
Robot Arm 2 end: [7.87299027835644;-22.3897327060213;0.385806847076026]
Robot Arm 1 togoal: 21.7770044200602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998768e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999729e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [0.847697624972206;-9.67795503778829;39.0613653853099]
Robot Arm 1 end: [-9.73024794709333;39.8780456168789;0.810616214866553]
Robot Arm 1 togoal: 35.8117988805402
Mobile Platform 2 Pose: [2.16484906986711;7.14099530344797;-22.8785666620715]
Robot Arm 2 end: [7.88063763597711;-22.6563246910867;0.390116372700458]
Robot Arm 1 togoal: 21.935784798676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000057e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999923e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [0.807872290851099;-9.65760371718317;39.2685688552559]
Robot Arm 1 end: [-9.49916682985505;40.0686328562451;0.623495931251474]
Robot Arm 1 togoal: 35.8489288885401
Mobile Platform 2 Pose: [2.16916186143732;7.13993271785692;-23.1464011404424]
Robot Arm 2 end: [7.85673948840919;-22.9077787884891;0.363992098178135]
Robot Arm 1 togoal: 22.0638293524574

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000142e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [0.7693422984878;-9.64050690426554;39.5278362862459]
Robot Arm 1 end: [-9.33774378517578;40.2438835068933;0.428922187642198]
Robot Arm 1 togoal: 35.9142514641177
Mobile Platform 2 Pose: [2.1806667865044;7.13554207975892;-23.4073037152888]
Robot Arm 2 end: [7.80363489613421;-23.1364542998652;0.320934713510796]
Robot Arm 1 togoal: 22.1579783324005

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000035e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992742e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [0.731206339306197;-9.62331973434094;39.8146934220295]
Robot Arm 1 end: [-9.24891529006402;40.4383624476023;0.27613315542407]
Robot Arm 1 togoal: 36.0355982577838
Mobile Platform 2 Pose: [2.19903843480077;7.1279028284184;-23.6571932636748]
Robot Arm 2 end: [7.75299924662407;-23.3656292798757;0.28163403769498]
Robot Arm 1 togoal: 22.2563486737207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [0.717909351910803;-9.61591276730974;40.0891252776683]
Robot Arm 1 end: [-9.23344612982111;40.6296261920444;0.144456236684948]
Robot Arm 1 togoal: 36.1938437227304
Mobile Platform 2 Pose: [2.23257892809959;7.11258834554677;-23.9069661828062]
Robot Arm 2 end: [7.64039271479852;-23.5603355758125;0.251536218600418]
Robot Arm 1 togoal: 22.286266945212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997914e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [0.700946850642897;-9.60521841792392;40.4054330597862]
Robot Arm 1 end: [-9.23210914373586;40.9181059078837;0.112404950713208]
Robot Arm 1 togoal: 36.4388176957014
Mobile Platform 2 Pose: [2.27466434753349;7.09207374181537;-24.1279660561858]
Robot Arm 2 end: [7.56549212039706;-23.763090658078;0.244150543099596]
Robot Arm 1 togoal: 22.3517230304465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997365e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995764e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [0.684442439988365;-9.59488442293511;40.7162369474847]
Robot Arm 1 end: [-9.24551172726832;41.2406097910504;0.11103180855428]
Robot Arm 1 togoal: 36.7199323537229
Mobile Platform 2 Pose: [2.3232125710244;7.06845673983064;-24.3174048041782]
Robot Arm 2 end: [7.47323631436603;-23.9400650403305;0.237095584946393]
Robot Arm 1 togoal: 22.3894575899756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998033e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993781e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [0.66993922928171;-9.58582082650276;41.0224584981819]
Robot Arm 1 end: [-9.22166290163264;41.5119446456852;0.102313899652585]
Robot Arm 1 togoal: 36.9389719714565
Mobile Platform 2 Pose: [2.38021900178563;7.04073990723717;-24.4658264919149]
Robot Arm 2 end: [7.38276182521855;-24.0755578799487;0.255615841854476]
Robot Arm 1 togoal: 22.4027439250585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998024e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [0.656565522834024;-9.57729583719482;41.3164479085388]
Robot Arm 1 end: [-9.2244703303808;41.7814749770293;0.0906405949224797]
Robot Arm 1 togoal: 37.1710685779536
Mobile Platform 2 Pose: [2.43631490610241;7.01338955621163;-24.5758065680098]
Robot Arm 2 end: [7.28576748531654;-24.1951404900064;0.24694950013879]
Robot Arm 1 togoal: 22.4036375565139

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998384e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991199e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [0.645033582536854;-9.56968505808736;41.5997076409448]
Robot Arm 1 end: [-9.23599353431554;42.023265721961;0.0727302662289527]
Robot Arm 1 togoal: 37.3846289265161
Mobile Platform 2 Pose: [2.49823678009808;6.98326799458789;-24.6463966543927]
Robot Arm 2 end: [7.19374762885953;-24.2734592207968;0.263419161629424]
Robot Arm 1 togoal: 22.3816466154235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [0.634536394079835;-9.56238573350752;41.8695282719684]
Robot Arm 1 end: [-9.26751542475983;42.2449496970776;0.0545878291972839]
Robot Arm 1 togoal: 37.5915455423836
Mobile Platform 2 Pose: [2.56954562605881;6.94840882060198;-24.6800422288415]
Robot Arm 2 end: [7.09341299484586;-24.3221229876655;0.290838052034438]
Robot Arm 1 togoal: 22.3343070880743

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998483e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991455e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [0.626703479760682;-9.55622313040553;42.1246386889941]
Robot Arm 1 end: [-9.33007796667254;42.4418659984215;0.0213080167298093]
Robot Arm 1 togoal: 37.7938574334754
Mobile Platform 2 Pose: [2.64481549564142;6.91145153558933;-24.6762974144418]
Robot Arm 2 end: [6.9988656647836;-24.3513786317348;0.291078628202617]
Robot Arm 1 togoal: 22.2808484825326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989519e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [0.615816095302024;-9.54780602171438;42.3745042287478]
Robot Arm 1 end: [-9.37878009428403;42.6753067542595;0.0403728645761868]
Robot Arm 1 togoal: 38.0184829542386
Mobile Platform 2 Pose: [2.71916627196475;6.87507881918587;-24.6475597885213]
Robot Arm 2 end: [6.91355880317293;-24.3638734843984;0.286287724124685]
Robot Arm 1 togoal: 22.2241725890614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998311e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986556e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [0.606524460201552;-9.54007676804605;42.6243985419125]
Robot Arm 1 end: [-9.45650454115827;42.8974598383324;0.0516084432668757]
Robot Arm 1 togoal: 38.2486280057443
Mobile Platform 2 Pose: [2.79770386452125;6.83683196463392;-24.6009624451809]
Robot Arm 2 end: [6.82594509315952;-24.372987969073;0.285400713024154]
Robot Arm 1 togoal: 22.1635267282784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982895e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [0.598072755717159;-9.53258933027033;42.87431095403]
Robot Arm 1 end: [-9.54932449098571;43.1215083353499;0.0717739251471188]
Robot Arm 1 togoal: 38.4877988287008
Mobile Platform 2 Pose: [2.8781177033749;6.79794465162867;-24.5406702846385]
Robot Arm 2 end: [6.76377035849779;-24.3681099604114;0.259936229961373]
Robot Arm 1 togoal: 22.1146300511455

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978156e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [0.589800270416518;-9.52497179629245;43.1242373672703]
Robot Arm 1 end: [-9.63307926877056;43.3331519005641;0.100415785270589]
Robot Arm 1 togoal: 38.7114308274836
Mobile Platform 2 Pose: [2.9538530858055;6.76182663764084;-24.4908751018577]
Robot Arm 2 end: [6.70469186066583;-24.4077554941715;0.240078492045673]
Robot Arm 1 togoal: 22.0968529082574

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997368e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [0.585717281515346;-9.51909476414372;43.3722295567386]
Robot Arm 1 end: [-9.70306763330037;43.5400788382808;0.10746974775812]
Robot Arm 1 togoal: 38.9248741862386
Mobile Platform 2 Pose: [3.03278410272094;6.72448913951594;-24.4580506242651]
Robot Arm 2 end: [6.6581870473267;-24.4688226972504;0.240848032623532]
Robot Arm 1 togoal: 22.101602091427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997037e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973885e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [0.58748572780334;-9.51598096558453;43.6114093523271]
Robot Arm 1 end: [-9.76729562290646;43.7573291118984;0.10331937746791]
Robot Arm 1 togoal: 39.1447705748788
Mobile Platform 2 Pose: [3.11324234687477;6.68650543875562;-24.4652090441322]
Robot Arm 2 end: [6.62363157830519;-24.564478143989;0.26086687758703]
Robot Arm 1 togoal: 22.1372330236676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997345e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962747e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [0.591368854563026;-9.51403451791695;43.8585732676982]
Robot Arm 1 end: [-9.83015676732783;43.9812156037255;0.116568664001698]
Robot Arm 1 togoal: 39.3691574548048
Mobile Platform 2 Pose: [3.18238550515121;6.65407678244355;-24.5335026571824]
Robot Arm 2 end: [6.58221953183899;-24.6251163514538;0.269810886977159]
Robot Arm 1 togoal: 22.1456430724468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998958e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [0.563762762539128;-9.49646401798922;44.1080868255697]
Robot Arm 1 end: [-9.78931898906623;44.1904123438734;0.0927759223861607]
Robot Arm 1 togoal: 39.5302576049155
Mobile Platform 2 Pose: [3.11590630457953;6.68959979787085;-24.6009103289037]
Robot Arm 2 end: [6.65292455772634;-24.7669593938007;0.241101623181697]
Robot Arm 1 togoal: 22.2939706180498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985644e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [0.57237849951891;-9.49779000427241;44.3599450209239]
Robot Arm 1 end: [-9.79717337782944;44.4852778901291;0.0625622811855085]
Robot Arm 1 togoal: 39.7905705600074
Mobile Platform 2 Pose: [3.0450244779535;6.72729215664129;-24.7255567103208]
Robot Arm 2 end: [6.7031777863165;-24.9648298272034;0.234944350809616]
Robot Arm 1 togoal: 22.4632192495036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999564e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [0.575679934702693;-9.49496119958884;44.6110595858173]
Robot Arm 1 end: [-9.80919834163994;44.8686886788134;0.0861258659257649]
Robot Arm 1 togoal: 40.1284020725854
Mobile Platform 2 Pose: [2.97126512594522;6.76635409431161;-24.8973290542938]
Robot Arm 2 end: [6.73512106248236;-25.2068709673184;0.243990614879344]
Robot Arm 1 togoal: 22.6482660387567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999381e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [0.598767802278506;-9.50515296885692;44.9095849081896]
Robot Arm 1 end: [-9.79958029084185;45.2169514178104;0.0852540428856371]
Robot Arm 1 togoal: 40.4267042103519
Mobile Platform 2 Pose: [2.8957884109335;6.80619820164859;-25.106750371168]
Robot Arm 2 end: [6.75237201342261;-25.4822284324819;0.265132271422109]
Robot Arm 1 togoal: 22.8455371804503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997464e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999743e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [0.632741824086446;-9.51959511134224;45.2078751490829]
Robot Arm 1 end: [-9.79160751071156;45.5441925065799;0.0780747534468715]
Robot Arm 1 togoal: 40.7084681862386
Mobile Platform 2 Pose: [2.84401561328723;6.83409737311491;-25.3457967854129]
Robot Arm 2 end: [6.75450611297476;-25.7876874401946;0.309448386909678]
Robot Arm 1 togoal: 23.0528177694218

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993433e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [0.679734070808714;-9.53728419221327;45.4916882386155]
Robot Arm 1 end: [-9.8100705820256;45.8753669950599;0.0930650572164886]
Robot Arm 1 togoal: 41.0063391647668
Mobile Platform 2 Pose: [2.78778995355789;6.86233524521187;-25.598851466327]
Robot Arm 2 end: [6.7242049152056;-26.1013452053328;0.361799068024664]
Robot Arm 1 togoal: 23.2442701384185

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999471e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [0.742383378106827;-9.55996145455068;45.7614345109525]
Robot Arm 1 end: [-9.83985174487538;46.1880403300626;0.113927813697909]
Robot Arm 1 togoal: 41.2937505238545
Mobile Platform 2 Pose: [2.72669703820128;6.89107568379869;-25.8561896983553]
Robot Arm 2 end: [6.67955132508498;-26.4050874737382;0.410672529463775]
Robot Arm 1 togoal: 23.4215105089098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999600e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [0.816718459073066;-9.58865317440357;46.0314659372295]
Robot Arm 1 end: [-9.87119318307629;46.4755334730749;0.122279635959944]
Robot Arm 1 togoal: 41.5606217388687
Mobile Platform 2 Pose: [2.7327733949977;6.89029666006225;-26.117693092077]
Robot Arm 2 end: [6.59281765952323;-26.6909553765449;0.465956336405302]
Robot Arm 1 togoal: 23.5589198902685

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996848e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [0.899372791815847;-9.62150404492029;46.3010935937296]
Robot Arm 1 end: [-9.92192785314814;46.7471215821229;0.128669682417414]
Robot Arm 1 togoal: 41.8231124818637
Mobile Platform 2 Pose: [2.74046419358591;6.88829043490812;-26.3840661328926]
Robot Arm 2 end: [6.50855258300106;-26.9568635539915;0.50805537081145]
Robot Arm 1 togoal: 23.6874141010833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997454e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [0.983657560010704;-9.65755194233231;46.5666501052182]
Robot Arm 1 end: [-9.97676522059297;47.0050034489118;0.131221568159417]
Robot Arm 1 togoal: 42.0757996123933
Mobile Platform 2 Pose: [2.74944025393241;6.88571657380617;-26.6451519800692]
Robot Arm 2 end: [6.45928943354398;-27.2308155113287;0.547282824625807]
Robot Arm 1 togoal: 23.8486338598302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997754e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997470e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [1.06469922394443;-9.69460954684057;46.8282653527592]
Robot Arm 1 end: [-10.0313869849655;47.2548446528002;0.131786623961359]
Robot Arm 1 togoal: 42.3215055078371
Mobile Platform 2 Pose: [2.68465125513434;6.91277586662754;-26.9085441863029]
Robot Arm 2 end: [6.45091156650792;-27.5013968255825;0.531290136241598]
Robot Arm 1 togoal: 24.0397776160678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998026e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997114e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [1.14256375654858;-9.73201089166722;47.0832084384083]
Robot Arm 1 end: [-10.0665919315176;47.4760723710912;0.109861639399677]
Robot Arm 1 togoal: 42.5336975065897
Mobile Platform 2 Pose: [2.69997285819006;6.90578806634232;-27.1954077293785]
Robot Arm 2 end: [6.41288038753796;-27.7785926353378;0.490596106703189]
Robot Arm 1 togoal: 24.2182313741577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997180e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [1.22575703377436;-9.77178797275793;47.307583963644]
Robot Arm 1 end: [-10.115045710091;47.6765049389702;0.100963996696584]
Robot Arm 1 togoal: 42.7334832489229
Mobile Platform 2 Pose: [2.71745665868516;6.89778521328353;-27.4882581214906]
Robot Arm 2 end: [6.3807697405024;-28.0668362736271;0.46528433309622]
Robot Arm 1 togoal: 24.4101357185648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997404e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [1.31562785486526;-9.81481361920422;47.4972996201239]
Robot Arm 1 end: [-10.1794254328179;47.8507221554448;0.10426069353527]
Robot Arm 1 togoal: 42.9172870618568
Mobile Platform 2 Pose: [2.73820131305246;6.88805141118486;-27.7880301576867]
Robot Arm 2 end: [6.34335669302061;-28.3491997249886;0.426739054866071]
Robot Arm 1 togoal: 24.5966633013274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [1.41253246184647;-9.86149107035318;47.6507330050348]
Robot Arm 1 end: [-10.2549970367891;47.9921847848576;0.115090733990998]
Robot Arm 1 togoal: 43.0772282134233
Mobile Platform 2 Pose: [2.67000006517503;6.91659056865903;-28.1004237877763]
Robot Arm 2 end: [6.35909742159232;-28.6400787088253;0.39319160401685]
Robot Arm 1 togoal: 24.8262875697999

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [1.43962733580681;-9.87357187693511;47.7693489485449]
Robot Arm 1 end: [-10.4057103878718;48.034764401604;0.172393437590676]
Robot Arm 1 togoal: 43.1839676173855
Mobile Platform 2 Pose: [2.60806006197163;6.93864743081711;-28.4012533029418]
Robot Arm 2 end: [6.37840163288249;-28.9418739889517;0.408911513058434]
Robot Arm 1 togoal: 25.0655814028743

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000023e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [1.46881252163904;-9.88722806517096;47.8758948660153]
Robot Arm 1 end: [-10.4882356093808;48.0315531211165;0.19965636914726]
Robot Arm 1 togoal: 43.219356545194
Mobile Platform 2 Pose: [2.55454093026492;6.95735661259007;-28.7010117733507]
Robot Arm 2 end: [6.39465689716559;-29.2423450371104;0.424308299975633]
Robot Arm 1 togoal: 25.3032675753131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000055e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [1.49605185982935;-9.89976947237036;47.9428970592511]
Robot Arm 1 end: [-10.5309394904939;47.9812589930056;0.212801408868444]
Robot Arm 1 togoal: 43.1949869152652
Mobile Platform 2 Pose: [2.51072557852677;6.97218851035042;-28.9996403688493]
Robot Arm 2 end: [6.40732585091116;-29.5414859938972;0.439494098153458]
Robot Arm 1 togoal: 25.5389833674598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [1.53210479668867;-9.91655021765274;47.9644352046967]
Robot Arm 1 end: [-10.5599193828714;47.9936170415381;0.224664719473987]
Robot Arm 1 togoal: 43.2192747057231
Mobile Platform 2 Pose: [2.47940394001884;6.98398485363196;-29.3076249738167]
Robot Arm 2 end: [6.41726442835483;-29.8498381673632;0.454551935686482]
Robot Arm 1 togoal: 25.7813878225148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998353e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [1.56738762996974;-9.93303873932574;47.9894469171658]
Robot Arm 1 end: [-10.5870166616863;48.0157918578155;0.235189024161643]
Robot Arm 1 togoal: 43.2513467474816
Mobile Platform 2 Pose: [2.4568403153894;6.99229881115066;-29.6139128033419]
Robot Arm 2 end: [6.42398231174653;-30.1563350857181;0.469572747971335]
Robot Arm 1 togoal: 26.0217320394055

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [1.60238434080626;-9.94955529499444;48.0169920577152]
Robot Arm 1 end: [-10.6154849030127;48.0393502743129;0.247566243034224]
Robot Arm 1 togoal: 43.2852402166327
Mobile Platform 2 Pose: [2.43123769778307;7.00328437037847;-29.910124262142]
Robot Arm 2 end: [6.43351964257979;-30.4524906118894;0.484599274138946]
Robot Arm 1 togoal: 26.2571717162577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [1.63751189414203;-9.96638482799684;48.0444545444083]
Robot Arm 1 end: [-10.6466821039149;48.0596662597794;0.26394849821796]
Robot Arm 1 togoal: 43.3174774910087
Mobile Platform 2 Pose: [2.39542964099145;7.02127653145812;-30.1852332564723]
Robot Arm 2 end: [6.45014311555627;-30.7272147778023;0.499686777286037]
Robot Arm 1 togoal: 26.4814599610052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999048e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [1.67317652827553;-9.98381469329502;48.0687075961918]
Robot Arm 1 end: [-10.6808405996341;48.0701642335561;0.286270791210001]
Robot Arm 1 togoal: 43.3423479855031
Mobile Platform 2 Pose: [2.38857386304259;7.02479842492404;-30.45943308827]
Robot Arm 2 end: [6.45274839164925;-31.0010948256387;0.514889828849606]
Robot Arm 1 togoal: 26.6974000829833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997872e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [1.71657512236637;-10.0055207497603;48.0852366752971]
Robot Arm 1 end: [-10.7489765642377;48.0749469070267;0.377154707125741]
Robot Arm 1 togoal: 43.3766065119232
Mobile Platform 2 Pose: [2.40642973591168;7.01597021513569;-30.7327231540941]
Robot Arm 2 end: [6.44342065677629;-31.2740829028402;0.530152482456074]
Robot Arm 1 togoal: 26.9064019344029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [1.7600085162758;-10.0276907109669;48.1147236435432]
Robot Arm 1 end: [-10.7930528582589;48.0830446591518;0.424002356380218]
Robot Arm 1 togoal: 43.4036028632118
Mobile Platform 2 Pose: [2.42195321031712;7.00915111240365;-31.017607978281]
Robot Arm 2 end: [6.4362662774911;-31.5584111835165;0.545431727320412]
Robot Arm 1 togoal: 27.1268810473452

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000037e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [1.80378797376802;-10.0505637739227;48.1409321532043]
Robot Arm 1 end: [-10.8217702225602;48.0815618800018;0.424353093408994]
Robot Arm 1 togoal: 43.4160581451062
Mobile Platform 2 Pose: [2.43363938611538;7.00602036329832;-31.3048411271841]
Robot Arm 2 end: [6.43295509418736;-31.8448228277146;0.560824797578999]
Robot Arm 1 togoal: 27.3525178716908

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000055e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [1.84741978675067;-10.0738239036703;48.1640177735332]
Robot Arm 1 end: [-10.8526709278834;48.074336999457;0.441628906070095]
Robot Arm 1 togoal: 43.4241184681189
Mobile Platform 2 Pose: [2.45272424089005;6.99948703447975;-31.5900977793545]
Robot Arm 2 end: [6.42651080688403;-32.1291028746235;0.576404424212188]
Robot Arm 1 togoal: 27.5757087329799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000045e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [1.88060473933728;-10.0922549907161;48.1811733884216]
Robot Arm 1 end: [-10.885544901311;48.0710374253219;0.494634096884043]
Robot Arm 1 togoal: 43.4357376296526
Mobile Platform 2 Pose: [2.48079320008872;6.98944482709533;-31.8684011343202]
Robot Arm 2 end: [6.41685055321363;-32.4062826205348;0.592170848523561]
Robot Arm 1 togoal: 27.7923947609857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000068e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [1.92334427352242;-10.1157893483054;48.1931932473563]
Robot Arm 1 end: [-10.9058072202323;48.048213137327;0.506171425525429]
Robot Arm 1 togoal: 43.4252135919739
Mobile Platform 2 Pose: [2.51852673864749;6.97564025689247;-32.1394093373043]
Robot Arm 2 end: [6.4037348572868;-32.6760186506434;0.608109028910093]
Robot Arm 1 togoal: 28.0021018682097

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000039e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [1.96432554561131;-10.1386230961939;48.193066497852]
Robot Arm 1 end: [-10.9193405700289;48.011544854687;0.517459926042281]
Robot Arm 1 togoal: 43.3993484375604
Mobile Platform 2 Pose: [2.56530724859598;6.9581317676493;-32.4041866774205]
Robot Arm 2 end: [6.3872232638191;-32.9393610689526;0.62420183886078]
Robot Arm 1 togoal: 28.2056664429659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [1.99728610300066;-10.1575153961744;48.1794818365686]
Robot Arm 1 end: [-10.9380036527513;47.9688880508801;0.653904496702785]
Robot Arm 1 togoal: 43.3680563479011
Mobile Platform 2 Pose: [2.61767009384875;6.93740625700387;-32.6681744995971]
Robot Arm 2 end: [6.36777711212241;-33.2017089698967;0.640437027095431]
Robot Arm 1 togoal: 28.4077246161173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [2.02800804031784;-10.1753077662801;48.1429794404414]
Robot Arm 1 end: [-10.9441619748295;47.9072777122502;0.689092869513765]
Robot Arm 1 togoal: 43.3164136772114
Mobile Platform 2 Pose: [2.67434264141595;6.91399579047616;-32.9312362728741]
Robot Arm 2 end: [6.34592558014325;-33.4629045356602;0.65682316199836]
Robot Arm 1 togoal: 28.6084598314629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998932e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [2.06216881311571;-10.1947353373374;48.0847919085083]
Robot Arm 1 end: [-10.9515021781941;47.8283491292469;0.666859155884366]
Robot Arm 1 togoal: 43.2513467761254
Mobile Platform 2 Pose: [2.73418589993508;6.88839832712107;-33.193190021711]
Robot Arm 2 end: [6.32216633873278;-33.7227444154008;0.673371820168538]
Robot Arm 1 togoal: 28.8079883039225

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998821e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [2.09328347032979;-10.2124826932474;48.0125143755729]
Robot Arm 1 end: [-10.9567259861993;47.7386729148762;0.635558502907409]
Robot Arm 1 togoal: 43.1761022335438
Mobile Platform 2 Pose: [2.79283983439972;6.86143669759096;-33.4609246900966]
Robot Arm 2 end: [6.29729393060396;-33.9880627702137;0.690097301165638]
Robot Arm 1 togoal: 29.0123998356418

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998858e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [2.11625735073688;-10.2259767858994;47.9281506878963]
Robot Arm 1 end: [-10.9641536647035;47.6482012115203;0.644050226880388]
Robot Arm 1 togoal: 43.1004853133348
Mobile Platform 2 Pose: [2.85287429565157;6.83342454052627;-33.725806497889]
Robot Arm 2 end: [6.271663093865;-34.2502351724223;0.70703820811161]
Robot Arm 1 togoal: 29.2147815250549

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [2.1376048319508;-10.2385517364089;47.8356524053727]
Robot Arm 1 end: [-10.9697643882477;47.5501833494178;0.646239160439763]
Robot Arm 1 togoal: 43.0175835513799
Mobile Platform 2 Pose: [2.91357116559287;6.80469447466397;-33.9875991693601]
Robot Arm 2 end: [6.2456161391397;-34.5090013900419;0.724209838939192]
Robot Arm 1 togoal: 29.4150653141909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998686e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [2.15725170037376;-10.2501422316131;47.7361646695851]
Robot Arm 1 end: [-10.9752398108428;47.4473078384237;0.641422514210444]
Robot Arm 1 togoal: 42.9305859035436
Mobile Platform 2 Pose: [2.97420092945537;6.77559352940176;-34.2460567355506]
Robot Arm 2 end: [6.21951204486556;-34.7640889231864;0.741629723084307]
Robot Arm 1 togoal: 29.6131742506196

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998666e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000987e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.34228860067617;9.99051092833632;9.81851213867258]
Robot Arm 2 end: [9.76841225043192;9.82919065759656;1.48915541841058]
Robot Arm 1 togoal: 27.9997846262381

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997560e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.755921590689592;-9.97988497819246;-9.6577319006169]
Robot Arm 1 end: [-9.69473139479082;-9.65242476486688;1.46645126276626]
Robot Arm 1 togoal: 27.8227104972978
Mobile Platform 2 Pose: [-2.32895894047377;9.9814267230956;9.63158205878716]
Robot Arm 2 end: [9.55528122715621;9.627190707333;1.39653872700415]
Robot Arm 1 togoal: 27.7065178169986

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975433e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.741041397102319;-9.96973107815118;-9.48856967281555]
Robot Arm 1 end: [-9.55458065538716;-9.4798216906108;1.39996957464794]
Robot Arm 1 togoal: 27.6018065725759
Mobile Platform 2 Pose: [-2.31567298299289;9.97238624003146;9.44708615007336]
Robot Arm 2 end: [9.39054503783781;9.35726058913946;1.25880539478662]
Robot Arm 1 togoal: 27.4001007368818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.726085750816405;-9.95952595608916;-9.32064150140879]
Robot Arm 1 end: [-9.42892607377525;-9.30814325800548;1.31005200249116]
Robot Arm 1 togoal: 27.3921898268382
Mobile Platform 2 Pose: [-2.30202883667075;9.96308974577543;9.27536895724026]
Robot Arm 2 end: [9.29152290111774;9.10148778508252;1.11625137731742]
Robot Arm 1 togoal: 27.1513077566361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970144e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.711071148155552;-9.94928082607102;-9.15383416545519]
Robot Arm 1 end: [-9.32182454994546;-9.13747218263378;1.19975272128423]
Robot Arm 1 togoal: 27.1970392744144
Mobile Platform 2 Pose: [-2.28801559780112;9.95354009229704;9.11622743409446]
Robot Arm 2 end: [9.24237240333583;8.84723468400171;0.96920916638411]
Robot Arm 1 togoal: 26.940520114997

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.696014147992118;-9.93900693215315;-8.9880018026876]
Robot Arm 1 end: [-9.23670284449871;-8.96787640810329;1.07283142514188]
Robot Arm 1 togoal: 27.0191029982884
Mobile Platform 2 Pose: [-2.27333142457925;9.94356901363159;8.97208807950476]
Robot Arm 2 end: [9.27418677386564;8.54215856982616;0.778431410460103]
Robot Arm 1 togoal: 26.7555102424268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.680930058676466;-9.92871465548676;-8.82297746775894]
Robot Arm 1 end: [-9.17623760051621;-8.79941220629422;0.933626381288688]
Robot Arm 1 togoal: 26.8605844236788
Mobile Platform 2 Pose: [-2.25885405983727;9.93372301502113;8.8588146559015]
Robot Arm 2 end: [9.36772416127122;8.30182751360118;0.6274526341635]
Robot Arm 1 togoal: 26.6620339887956

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.665831815598383;-9.91841275221804;-8.6585868348378]
Robot Arm 1 end: [-9.14226012173675;-8.63212919847478;0.786906130707495]
Robot Arm 1 togoal: 26.7230472597311
Mobile Platform 2 Pose: [-2.22059825965257;9.91202987768147;8.77441578452093]
Robot Arm 2 end: [9.48109460500665;8.13353768747954;0.498761795072682]
Robot Arm 1 togoal: 26.6342487601973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.650729121699015;-9.90810777132205;-8.49466311568652]
Robot Arm 1 end: [-9.13568978254168;-8.4660764421974;0.6377047583955]
Robot Arm 1 togoal: 26.6073450969199
Mobile Platform 2 Pose: [-2.18437098774291;9.89142669280607;8.71496900301727]
Robot Arm 2 end: [9.61436901836773;8.04525031356835;0.368929810795842]
Robot Arm 1 togoal: 26.6773648734071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.635627878258829;-9.89780367077227;-8.33106222595957]
Robot Arm 1 end: [-9.14979122173426;-8.30126073263887;0.493056253941508]
Robot Arm 1 togoal: 26.508627210497
Mobile Platform 2 Pose: [-2.1511923857707;9.87252023047238;8.67758321204528]
Robot Arm 2 end: [9.74552992323083;8.03995732714395;0.244234568004528]
Robot Arm 1 togoal: 26.776036717235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000285e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.62042175742754;-9.88742786307749;-8.16786274884478]
Robot Arm 1 end: [-9.17946487028817;-8.13806180322454;0.357255935201147]
Robot Arm 1 togoal: 26.4233884832465
Mobile Platform 2 Pose: [-2.13992799489362;9.86482324116443;8.65856600911252]
Robot Arm 2 end: [9.78824491934155;8.0337161591272;0.219140084872255]
Robot Arm 1 togoal: 26.8045592154474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.605140705194117;-9.87700076826651;-8.00514089229303]
Robot Arm 1 end: [-9.18559970049075;-7.97434911767715;0.318955631894597]
Robot Arm 1 togoal: 26.3174727649618
Mobile Platform 2 Pose: [-2.1281992103244;9.85681712487186;8.64454418333784]
Robot Arm 2 end: [9.85666271864977;8.05104920061562;0.179468831484304]
Robot Arm 1 togoal: 26.8687018190563

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.589843037460278;-9.86656233439354;-7.84333051322093]
Robot Arm 1 end: [-9.17526176801076;-7.81073011971916;0.318918031915389]
Robot Arm 1 togoal: 26.1984307226063
Mobile Platform 2 Pose: [-2.11665525290249;9.84893702718916;8.63889931871262]
Robot Arm 2 end: [9.91763164865026;8.06324943227755;0.16283680719701]
Robot Arm 1 togoal: 26.9228088765321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996663e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.574476956223845;-9.85607729661586;-7.68269096600947]
Robot Arm 1 end: [-9.16488386391496;-7.64827828656266;0.318876634429779]
Robot Arm 1 togoal: 26.0806365064842
Mobile Platform 2 Pose: [-2.09342215993135;9.83527383367471;8.64088346543526]
Robot Arm 2 end: [9.99733104290512;8.10485988559695;0.147537160307614]
Robot Arm 1 togoal: 27.0104720523106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.559043394587716;-9.84554629251811;-7.52323027028225]
Robot Arm 1 end: [-9.15446666727764;-7.48700179297147;0.318831395976652]
Robot Arm 1 togoal: 25.9640912486406
Mobile Platform 2 Pose: [-2.07246709264785;9.8230685494739;8.6578748262216]
Robot Arm 2 end: [10.0143945757092;8.15391428979239;0.131320975574558]
Robot Arm 1 togoal: 27.0568254247285

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988958e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.543543302255094;-9.83496997099686;-7.36495605264821]
Robot Arm 1 end: [-9.14401086728869;-7.3269084214144;0.318782272337449]
Robot Arm 1 togoal: 25.8487956198283
Mobile Platform 2 Pose: [-2.04435655925512;9.80743973129558;8.6794687362289]
Robot Arm 2 end: [10.0100440395967;8.19581314565622;0.125845181973563]
Robot Arm 1 togoal: 27.0820223665903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.527977644896094;-9.82434899182775;-7.20787553909628]
Robot Arm 1 end: [-9.13351716184729;-7.16800555442587;0.318729221279988]
Robot Arm 1 togoal: 25.7347498282938
Mobile Platform 2 Pose: [-2.03637837577113;9.80197616875581;8.70325913283808]
Robot Arm 2 end: [10.0076284320402;8.24044712607487;0.117153862754784]
Robot Arm 1 togoal: 27.1106969812798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994098e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.512347403485289;-9.81368402521274;-7.05199554786409]
Robot Arm 1 end: [-9.12298625740851;-7.01030016733248;0.318672201689868]
Robot Arm 1 togoal: 25.6219536202526
Mobile Platform 2 Pose: [-2.02878238458981;9.79677254830704;8.72792177633533]
Robot Arm 2 end: [10.0034068320667;8.28105713989813;0.11141359487336]
Robot Arm 1 togoal: 27.1352219241876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.496653573622041;-9.80297575131568;-6.89732248276789]
Robot Arm 1 end: [-9.11241886850294;-6.85379882146527;0.318611173604233]
Robot Arm 1 togoal: 25.5104062807587
Mobile Platform 2 Pose: [-2.02143400314477;9.79173720365611;8.75280495942826]
Robot Arm 2 end: [9.99826207173679;8.31689802895629;0.108035309306644]
Robot Arm 1 togoal: 27.1557674999289

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.480897164832203;-9.79222485978517;-6.74386232701583]
Robot Arm 1 end: [-9.10181571724627;-6.69850765785465;0.318546098244226]
Robot Arm 1 togoal: 25.4001066352272
Mobile Platform 2 Pose: [-2.01422212170279;9.78679500185357;8.77750995525568]
Robot Arm 2 end: [9.99344399067887;8.34947819012445;0.105980471930425]
Robot Arm 1 togoal: 27.1743156808609

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.465079199852494;-9.78143204926559;-6.59162063751894]
Robot Arm 1 end: [-9.09117753283955;-6.54443239142245;0.318476938046057]
Robot Arm 1 togoal: 25.2910530516085
Mobile Platform 2 Pose: [-2.00710193509676;9.78191532070189;8.80202819398204]
Robot Arm 2 end: [9.98849451922622;8.3793320640706;0.104734092419495]
Robot Arm 1 togoal: 27.190909682971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000144e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.449200713898937;-9.7705980268973;-6.44060253971254]
Robot Arm 1 end: [-9.08050505106126;-6.39157830568398;0.31840365669065]
Robot Arm 1 togoal: 25.1832434432081
Mobile Platform 2 Pose: [-2.00003383957457;9.77707108500934;8.82639473499189]
Robot Arm 2 end: [9.98321844104064;8.40721820583536;0.103933593221323]
Robot Arm 1 togoal: 27.2059327820386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.964034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.433262753920773;-9.75972350780692;-6.29081272289949]
Robot Arm 1 end: [-9.06979901375214;-6.239950247972;0.318326219131776]
Robot Arm 1 togoal: 25.0766752721467
Mobile Platform 2 Pose: [-1.99298694802521;9.77224125337441;8.85065081831992]
Robot Arm 2 end: [9.97763254903793;8.4338692657412;0.103358087982619]
Robot Arm 1 togoal: 27.2199016706807

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.281349e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.417266377841304;-9.74880921458877;-6.14225543612552]
Robot Arm 1 end: [-9.05906016829367;-6.08955262519274;0.318244591622641]
Robot Arm 1 togoal: 24.9713455534512
Mobile Platform 2 Pose: [-1.9859411237119;9.76741210114263;8.87482748187172]
Robot Arm 2 end: [9.9718459505105;8.45990604878309;0.102875371064422]
Robot Arm 1 togoal: 27.2333222259792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.278083e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.401212653787188;-9.73785587677836;-5.99493448459504]
Robot Arm 1 end: [-9.04828926708141;-5.94038940012288;0.318158741740877]
Robot Arm 1 togoal: 24.8672508597659
Mobile Platform 2 Pose: [-1.97888651707463;9.76257690200784;8.89894110561571]
Robot Arm 2 end: [9.96597515250383;8.48583458494225;0.102398995739151]
Robot Arm 1 togoal: 27.246626816939

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.607776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.385102659307702;-9.72686423031915;-5.8488532266347]
Robot Arm 1 end: [-9.03748706699411;-5.79246408825485;0.318068638411877]
Robot Arm 1 togoal: 24.7643873266729
Mobile Platform 2 Pose: [-1.97182200360251;9.75773491181667;8.92299280731342]
Robot Arm 2 end: [9.96008994949963;8.51204591574984;0.101865500382116]
Robot Arm 1 togoal: 27.2601353171064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.251025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.368937480585549;-9.71583501702348;-5.70401457121015]
Robot Arm 1 end: [-9.02665432885965;-5.64577975519625;0.31797425193045]
Robot Arm 1 togoal: 24.6627506586054
Mobile Platform 2 Pose: [-1.96475379085344;9.7528903380366;8.94697039058276]
Robot Arm 2 end: [9.95417577643136;8.53878315055359;0.101229880556756]
Robot Arm 1 togoal: 27.2740051846448

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.352718211640781;-9.7047689840289;-5.56042097600019]
Robot Arm 1 end: [-9.01579181691897;-5.50033901462767;0.317875553980748]
Robot Arm 1 togoal: 24.5623361353394
Mobile Platform 2 Pose: [-1.95769225179259;9.74805025331722;8.9708521240196]
Robot Arm 2 end: [9.94813717344102;8.56609330590869;0.100473325965436]
Robot Arm 1 togoal: 27.2882006252844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.336445953529443;-9.69366688325088;-5.41807444603074]
Robot Arm 1 end: [-9.00490029828894;-5.35614402682166;0.317772517654445]
Robot Arm 1 togoal: 24.4631386190461
Mobile Platform 2 Pose: [-1.9506482971875;9.74322210194341;8.99461457366349]
Robot Arm 2 end: [9.94192155320171;8.59388963441943;0.0995990369180029]
Robot Arm 1 togoal: 27.3026252889511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994958e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.32012181353854;-9.68252947083301;-5.27697653286952]
Robot Arm 1 end: [-8.99398054242525;-5.213196497724;0.317665117467128]
Robot Arm 1 togoal: 24.3651525618846
Mobile Platform 2 Pose: [-1.94362994634322;9.73841174192007;9.01825978622992]
Robot Arm 2 end: [9.93603695125531;8.62252046086889;0.0985346365779305]
Robot Arm 1 togoal: 27.3178924031015

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994180e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.303746904378936;-9.67135750659591;-5.13712833438094]
Robot Arm 1 end: [-8.98303332058644;-5.07149767859683;0.317553329372896]
Robot Arm 1 togoal: 24.2683720141152
Mobile Platform 2 Pose: [-1.93666106854705;9.73363545638124;9.04197876661937]
Robot Arm 2 end: [9.93041526430368;8.65175750318924;0.0973732817885877]
Robot Arm 1 togoal: 27.3337927042365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993715e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.287322343377793;-9.66015175348576;-4.99853049503885]
Robot Arm 1 end: [-8.97205940530007;-4.93104836622174;0.317437130777141]
Robot Arm 1 togoal: 24.1727906327106
Mobile Platform 2 Pose: [-1.9297567488391;9.7289034905471;9.06594889547179]
Robot Arm 2 end: [9.92494231180846;8.68136070632829;0.0962088191316423]
Robot Arm 1 togoal: 27.3500748422728

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.270849251672138;-9.64891297702367;-4.86118320679359]
Robot Arm 1 end: [-8.961059569832;-4.79184890365925;0.3173165005475]
Robot Arm 1 togoal: 24.0784016904415
Mobile Platform 2 Pose: [-1.92292239654724;9.7242194468684;9.09028781546155]
Robot Arm 2 end: [9.91949813885361;8.71111135888029;0.0951158318383146]
Robot Arm 1 togoal: 27.3664984981496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.254328753405152;-9.63764194475692;-4.72508621048801]
Robot Arm 1 end: [-8.95003458765994;-4.65389918155965;0.317191419022983]
Robot Arm 1 togoal: 23.9851980854121
Mobile Platform 2 Pose: [-1.91615537002393;9.71958137530391;9.11503945123402]
Robot Arm 2 end: [9.91392296346025;8.74075711477416;0.0941504661571568]
Robot Arm 1 togoal: 27.3827720404968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993948e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.237761974926717;-9.62633942571313;-4.59023879781584]
Robot Arm 1 end: [-8.93898523195212;-4.51719864001875;0.317061868021266]
Robot Arm 1 togoal: 23.8931723510189
Mobile Platform 2 Pose: [-1.90944479019169;9.71498176537998;9.14017023754153]
Robot Arm 2 end: [9.90814159831604;8.77005877051484;0.0933470467014699]
Robot Arm 1 togoal: 27.398675170567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.221150043999774;-9.61500618985849;-4.45663981381407]
Robot Arm 1 end: [-8.92791227505214;-4.38174627097048;0.316927830844163]
Robot Arm 1 togoal: 23.8023166663075
Mobile Platform 2 Pose: [-1.90283591850164;9.7104402817633;9.16560074933919]
Robot Arm 2 end: [9.8895574120106;8.77899607462204;0.0973767482509719]
Robot Arm 1 togoal: 27.391091449215

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987438e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.204494089013975;-9.60364300756088;-4.3242876598801]
Robot Arm 1 end: [-8.91681648797091;-4.24754062110698;0.316789292281287]
Robot Arm 1 togoal: 23.7126228666979
Mobile Platform 2 Pose: [-1.89567653615235;9.70553352038382;9.18487624035217]
Robot Arm 2 end: [9.88432054566256;8.80171460932849;0.0966677160936469]
Robot Arm 1 togoal: 27.4029085731473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.627083e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.187795238208097;-9.59225064905909;-4.19318029730241]
Robot Arm 1 end: [-8.90569863988658;-4.11457979531553;0.316646238611902]
Robot Arm 1 togoal: 23.6240824550523
Mobile Platform 2 Pose: [-1.8885520851913;9.70065089913509;9.20470087451348]
Robot Arm 2 end: [9.87945057367856;8.82423597699584;0.0960476515652183]
Robot Arm 1 togoal: 27.414866485618
Step 41:
Mobile Platform 1 Pose: [0.17105461890263;-9.58082988393898;-4.06331525129281]
Robot Arm 1 end: [-8.89455949765334;-3.98286146062012;0.31649865760501]
Robot Arm 1 togoal: 23.5366866130534
Mobile Platform 2 Pose: [-1.88145908627845;9.69578867702437;9.22520189931317]
Robot Arm 2 end: [9.87231143310244;8.84328422089772;0.0958476562263409]
Robot Arm 1 togoal: 27.4227858017144
Step 42:
Mobile Platform 1 Pose: [0.15427335674391;-9.56938148061755;-3.93468961550677]
Robot Arm 1 end: [-8.88339982532;-3.85238285061453;0.316346538517676]
Robot Arm 1 togoal: 23.4504262128678
Mobile Platform 2 Pose: [-1.87429031044648;9.69087472163293;9.24576354601135]
Robot Arm 2 end: [9.86510259307721;8.86240333263859;0.0956506207647865]
Robot Arm 1 togoal: 27.4307160199931
Step 43:
Mobile Platform 1 Pose: [0.137452574961122;-9.55790620583581;-3.80730005703718]
Robot Arm 1 end: [-8.87222038365899;-3.72314077037239;0.316189872091636]
Robot Arm 1 togoal: 23.3652918290605
Mobile Platform 2 Pose: [-1.867045440146;9.68590881723721;9.26637847918431]
Robot Arm 2 end: [9.85782386890482;8.88158623622635;0.0954565461453247]
Robot Arm 1 togoal: 27.4386523310334
Step 44:
Mobile Platform 1 Pose: [0.120593393637415;-9.54640482416134;-3.68114282186597]
Robot Arm 1 end: [-8.86102192970673;-3.59513160181865;0.316028650548207]
Robot Arm 1 togoal: 23.2812737507346
Mobile Platform 2 Pose: [-1.85972416667844;9.68089075417281;9.28703923917493]
Robot Arm 2 end: [9.85047508631715;8.9008257327297;0.0952654327550653]
Robot Arm 1 togoal: 27.4465898353102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.190713e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.106160153381712;-9.53610971050027;-3.55621374062943]
Robot Arm 1 end: [-8.87958628780164;-3.46245450815709;0.273064602582872]
Robot Arm 1 togoal: 23.2214082806981
Mobile Platform 2 Pose: [-1.85232619046807;9.67582032902008;9.30773824329261]
Robot Arm 2 end: [9.84305608172577;8.92011450139119;0.0950772804028775]
Robot Arm 1 togoal: 27.4545235436809

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.453895e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.0914284567795207;-9.52565773130469;-3.43222024059079]
Robot Arm 1 end: [-8.91737586558969;-3.32861543029679;0.211507452395699]
Robot Arm 1 togoal: 23.1783060136191
Mobile Platform 2 Pose: [-1.84485122133283;9.67069734478891;9.32846778713354]
Robot Arm 2 end: [9.83556670246912;8.93944510085763;0.0948920883193518]
Robot Arm 1 togoal: 27.4624483779626

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.712075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.0774313906136508;-9.51556457511557;-3.3096066023755]
Robot Arm 1 end: [-8.93447195652737;-3.1903839363884;0.184798014392775]
Robot Arm 1 togoal: 23.1146250760897
Mobile Platform 2 Pose: [-1.83729897875363;9.66552161110248;9.34922004602253]
Robot Arm 2 end: [9.82800680705693;8.95880997052938;0.0947098551573116]
Robot Arm 1 togoal: 27.4703591715983

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994704e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.0629279378201709;-9.50520437106988;-3.1906157881846]
Robot Arm 1 end: [-8.97119956132962;-3.06313986778462;0.14077619906974]
Robot Arm 1 togoal: 23.0750309586304
Mobile Platform 2 Pose: [-1.82966919214201;9.66029294437978;9.36998707657781]
Robot Arm 2 end: [9.82037626541141;8.9782014320297;0.0945305789928926]
Robot Arm 1 togoal: 27.4782506704165

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.938972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.0840131979701421;-9.51262675935738;-3.07427397448468]
Robot Arm 1 end: [-8.92490369423243;-2.84585049061311;0.230161487536121]
Robot Arm 1 togoal: 22.9092583626886
Mobile Platform 2 Pose: [-1.82196160110554;9.65501116801674;9.39076081839992]
Robot Arm 2 end: [9.81267495910504;8.99761169079545;0.0943542573271989]
Robot Arm 1 togoal: 27.4861175334837

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.0699285140257704;-9.50247221314094;-2.98116101803755]
Robot Arm 1 end: [-8.96803749164897;-2.76092553991376;0.169990033023661]
Robot Arm 1 togoal: 22.9009365496412
Mobile Platform 2 Pose: [-1.81417595571089;9.64967611256559;9.41153309588546]
Robot Arm 2 end: [9.80490278159447;9.01703283778996;0.0941808870885507]
Robot Arm 1 togoal: 27.4939543340524

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996644e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.0559107919982058;-9.49234900665943;-2.88930332059987]
Robot Arm 1 end: [-9.01034931298453;-2.67645182814775;0.122207760462838]
Robot Arm 1 togoal: 22.8919730840841
Mobile Platform 2 Pose: [-1.80631201674405;9.64428761591241;9.43229562016667]
Robot Arm 2 end: [9.79705963845027;9.03645685133897;0.0940104646353325]
Robot Arm 1 togoal: 27.5017555606054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.901463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.0758853328462604;-9.49921957820416;-2.79962260984689]
Robot Arm 1 end: [-8.96910722850244;-2.48942699127284;0.213467782408165]
Robot Arm 1 togoal: 22.7491198976312
Mobile Platform 2 Pose: [-1.79836955596755;9.63884552345247;9.45303999117724]
Robot Arm 2 end: [9.78914544758221;9.05587559909017;0.0938429857594528]
Robot Arm 1 togoal: 27.5095156179977

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996586e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.0633325594254091;-9.48990913156499;-2.7284326874384]
Robot Arm 1 end: [-8.99012217113483;-2.42057401288323;0.182965190669552]
Robot Arm 1 togoal: 22.7307222946196
Mobile Platform 2 Pose: [-1.79033649679066;9.63334375847151;9.47375771429604]
Robot Arm 2 end: [9.78548020268829;9.09165173989496;0.0915804550133988]
Robot Arm 1 togoal: 27.5317931352826

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992809e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.0504404178419869;-9.48043840987083;-2.65957677002716]
Robot Arm 1 end: [-9.02928406446591;-2.36983088330118;0.132980317464467]
Robot Arm 1 togoal: 22.7387987089625
Mobile Platform 2 Pose: [-1.78257203946443;9.62802720485914;9.49588100057142]
Robot Arm 2 end: [9.78363841103051;9.13030313631692;0.0894370053376261]
Robot Arm 1 togoal: 27.5573986340051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.912867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.0633891398499559;-9.48389192360167;-2.59227850987223]
Robot Arm 1 end: [-8.97638848397227;-2.21957968389372;0.232906670126465]
Robot Arm 1 togoal: 22.6070925147868
Mobile Platform 2 Pose: [-1.77515241421746;9.62294266615611;9.52022208236356]
Robot Arm 2 end: [9.77628620818762;9.15326758695528;0.0892317996361939]
Robot Arm 1 togoal: 27.5680814643536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996868e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.0525177853667974;-9.47552562862152;-2.53478994227867]
Robot Arm 1 end: [-8.9836818358364;-2.15833463008124;0.21879449769347]
Robot Arm 1 togoal: 22.5809879365703
Mobile Platform 2 Pose: [-1.76765625513838;9.61780589461908;9.54457189706319]
Robot Arm 2 end: [9.76886469710764;9.17625223523457;0.0890298515834682]
Robot Arm 1 togoal: 27.5787454338997

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997260e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.0415724419967307;-9.46714261139419;-2.47938439056832]
Robot Arm 1 end: [-8.99891280865481;-2.10848258587545;0.192110092144295]
Robot Arm 1 togoal: 22.5685619132128
Mobile Platform 2 Pose: [-1.76008331469368;9.61261672237394;9.56892273130067]
Robot Arm 2 end: [9.76137376373022;9.19924961625957;0.0888311621767537]
Robot Arm 1 togoal: 27.5893853943382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821021e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.0247283799319698;-9.45582484470846;-2.4255270515723]
Robot Arm 1 end: [-8.86917836401994;-2.12540104659595;0.279314346321392]
Robot Arm 1 togoal: 22.4635933442657
Mobile Platform 2 Pose: [-1.75243335547189;9.60737498846962;9.59326676784283]
Robot Arm 2 end: [9.75381330524944;9.22225216179137;0.0886357319399564]
Robot Arm 1 togoal: 27.5999961171475

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.00756429908528577;-9.44414339537032;-2.34593045609028]
Robot Arm 1 end: [-8.84656958878855;-2.11931970204628;0.248070085574548]
Robot Arm 1 togoal: 22.4430712219272
Mobile Platform 2 Pose: [-1.74470615041705;9.60208053903731;9.61759608751519]
Robot Arm 2 end: [9.74618323031816;9.24525220207496;0.0884435609266377]
Robot Arm 1 togoal: 27.6105722946465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.760060e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-0.00975370311550661;-9.43234409385856;-2.25299972330699]
Robot Arm 1 end: [-8.83653380141168;-2.06307719335155;0.227695047467585]
Robot Arm 1 togoal: 22.4054786765364
Mobile Platform 2 Pose: [-1.73690148305885;9.59673322744739;9.64190267123398]
Robot Arm 2 end: [9.73848345924789;9.26824196777447;0.0882546487235329]
Robot Arm 1 togoal: 27.6211085411377

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.477366e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-0.0273903293904207;-9.42037486167026;-2.15334851966647]
Robot Arm 1 end: [-8.75309614211307;-2.03343477594017;0.312371173107691]
Robot Arm 1 togoal: 22.3146353558228
Mobile Platform 2 Pose: [-1.7290191477392;9.59133291446384;9.6661784021482]
Robot Arm 2 end: [9.73071392420404;9.29121359201632;0.0880689944545314]
Robot Arm 1 togoal: 27.6315993941398

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.118191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-0.0447453204263212;-9.40853603955538;-2.04403099143278]
Robot Arm 1 end: [-8.77567445501428;-1.92736197140872;0.254196097473513]
Robot Arm 1 togoal: 22.2798722385282
Mobile Platform 2 Pose: [-1.72105894983491;9.58587946839607;9.69041506789146]
Robot Arm 2 end: [9.7228745693962;9.31415911253994;0.0878865967851266]
Robot Arm 1 togoal: 27.6420393157075

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.124626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-0.0621023586284466;-9.39669535010071;-1.93185940388684]
Robot Arm 1 end: [-8.79166412637866;-1.81915855435704;0.215438140028055]
Robot Arm 1 togoal: 22.2378875197684
Mobile Platform 2 Pose: [-1.71302070597616;9.58037276524781;9.71460436294306]
Robot Arm 2 end: [9.71496535126318;9.33707047395581;0.0877074539273374]
Robot Arm 1 togoal: 27.6524226938413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-0.0792722910758953;-9.38494646725417;-1.81784877147486]
Robot Arm 1 end: [-8.73821095944495;-1.61853304928652;0.305904977298615]
Robot Arm 1 togoal: 22.0813793974068
Mobile Platform 2 Pose: [-1.70490424426056;9.57481268886302;9.73873789109794]
Robot Arm 2 end: [9.70698623865239;9.35993953011031;0.0875315636451013]
Robot Arm 1 togoal: 27.6627438439861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-0.0966337326074473;-9.3731039945263;-1.72490841382258]
Robot Arm 1 end: [-8.77384424696616;-1.52526144935172;0.23407501757439]
Robot Arm 1 togoal: 22.0668315622259
Mobile Platform 2 Pose: [-1.69670940446232;9.56919913106844;9.76280716804459]
Robot Arm 2 end: [9.69893721299354;9.38275804655657;0.0873589232601443]
Robot Arm 1 togoal: 27.6729970106191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-0.113953833123545;-9.36128571109517;-1.63095899334965]
Robot Arm 1 end: [-8.81327592960018;-1.42143528466334;0.179267480411599]
Robot Arm 1 togoal: 22.0496889166071
Mobile Platform 2 Pose: [-1.68843603823649;9.5635319918127;9.78680362405001]
Robot Arm 2 end: [9.69081826846605;9.40551770313069;0.0871895296583265]
Robot Arm 1 togoal: 27.6831763689273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-0.131280951846231;-9.34947081889326;-1.53949668927353]
Robot Arm 1 end: [-8.87133028516919;-1.35677608507478;0.105265354017959]
Robot Arm 1 togoal: 22.0705182169858
Mobile Platform 2 Pose: [-1.68008400931765;9.55781117930174;9.81071860675062]
Robot Arm 2 end: [9.68262941216011;9.42821009663192;0.0870233792964607]
Robot Arm 1 togoal: 27.6932760265741

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-0.14859095536876;-9.33766669620624;-1.44574934932374]
Robot Arm 1 end: [-8.92477614919062;-1.2872932970764;0.0507436224586765]
Robot Arm 1 togoal: 22.0841978818996
Mobile Platform 2 Pose: [-1.67165319371312;9.5520366101304;9.8345433840477]
Robot Arm 2 end: [9.67437066423109;9.45082674360584;0.0868604682096091]
Robot Arm 1 togoal: 27.7032900255542

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-0.165816156441667;-9.32591982507218;-1.35172537156802]
Robot Arm 1 end: [-8.98104042558451;-1.2178463435325;0.00653105422527428]
Robot Arm 1 togoal: 22.1000811856539
Mobile Platform 2 Pose: [-1.66314347989003;9.54620820940983;9.85826914707766]
Robot Arm 2 end: [9.66604205816923;9.47335908328623;0.0867007920232329]
Robot Arm 1 togoal: 27.7132123442644

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000055e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-0.182902134508833;-9.3142641784637;-1.25904469347806]
Robot Arm 1 end: [-8.9516444587261;-1.10498411533076;0.0106804974758668]
Robot Arm 1 togoal: 22.0174150365436
Mobile Platform 2 Pose: [-1.65455476895732;9.54032591089142;9.88188701334678]
Robot Arm 2 end: [9.6576436407785;9.49579848049771;0.0865443459545465]
Robot Arm 1 togoal: 27.7230368993629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000080e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-0.195505341914295;-9.30495584407262;-1.16977574433084]
Robot Arm 1 end: [-8.93458438815141;-0.999546218510695;0.00780795872490053]
Robot Arm 1 togoal: 21.9499032598998
Mobile Platform 2 Pose: [-1.64588697484175;9.53438965708667;9.9053880300895]
Robot Arm 2 end: [9.64917547165381;9.51813622844928;0.0863911248014159]
Robot Arm 1 togoal: 27.732757547221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001856e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-0.214368355358035;-9.29268669725407;-1.0807118137515]
Robot Arm 1 end: [-8.9145018867503;-0.905208413968071;0.00968578189760078]
Robot Arm 1 togoal: 21.8852923864818
Mobile Platform 2 Pose: [-1.63714206039592;9.52840041734895;9.92876320931051]
Robot Arm 2 end: [9.64333973284588;9.55614108196521;0.0854991936345887]
Robot Arm 1 togoal: 27.7554339264009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004320e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000130e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-0.23878405602552;-9.2777525480206;-0.98833072158708]
Robot Arm 1 end: [-8.88889014201751;-0.821207742809954;0.0162101112689025]
Robot Arm 1 togoal: 21.8209326202957
Mobile Platform 2 Pose: [-1.62503739033317;9.52077709383697;9.95219244266566]
Robot Arm 2 end: [9.60552157337279;9.57664320471313;0.0824840237977805]
Robot Arm 1 togoal: 27.7433012372792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004970e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000219e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-0.267660618464327;-9.2606415057408;-0.889649680838132]
Robot Arm 1 end: [-8.85641126281565;-0.750539527001221;0.0257963154036076]
Robot Arm 1 togoal: 21.7571592790163
Mobile Platform 2 Pose: [-1.61148193043893;9.51235711376938;9.97340988205564]
Robot Arm 2 end: [9.56228562876953;9.58753396873068;0.0806643358661626]
Robot Arm 1 togoal: 27.7205532560791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000884e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000266e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-0.284859378726064;-9.24939333832168;-0.781419084623189]
Robot Arm 1 end: [-8.96422162364943;-0.692210409165825;-0.0423440520533807]
Robot Arm 1 togoal: 21.8267979751923
Mobile Platform 2 Pose: [-1.59714467922025;9.50343903278318;9.99103226062136]
Robot Arm 2 end: [9.51908154041041;9.59302764099797;0.0802197966048127]
Robot Arm 1 togoal: 27.6939907546131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000086e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-0.300661407482826;-9.23916976920783;-0.662387784524996]
Robot Arm 1 end: [-9.06632619094011;-0.616351672341784;-0.0810397874829221]
Robot Arm 1 togoal: 21.8814590174157
Mobile Platform 2 Pose: [-1.58230189085709;9.49415376585598;10.0043595047504]
Robot Arm 2 end: [9.4835666231822;9.64810191603966;0.0503780622472095]
Robot Arm 1 togoal: 27.7095652464426

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995501e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000184e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-0.315032900083582;-9.23004242894768;-0.532822765655759]
Robot Arm 1 end: [-9.11953004519133;-0.510081260959665;-0.0862215281770083]
Robot Arm 1 togoal: 21.8769604447598
Mobile Platform 2 Pose: [-1.56680396902838;9.48451353508185;10.0147025846025]
Robot Arm 2 end: [9.44783416911982;9.67934495691459;0.034619585321905]
Robot Arm 1 togoal: 27.7074754992216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995708e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000385e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-0.328547651726112;-9.2215279380505;-0.398085697573204]
Robot Arm 1 end: [-9.16551108769554;-0.394181645002038;-0.0843810157319574]
Robot Arm 1 togoal: 21.8616805284395
Mobile Platform 2 Pose: [-1.55066160484519;9.47448865297026;10.0210976602546]
Robot Arm 2 end: [9.4098410011136;9.67881035744853;0.0335579432008336]
Robot Arm 1 togoal: 27.6804983453201

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997863e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000451e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-0.341892868565327;-9.21326682895079;-0.25970208306018]
Robot Arm 1 end: [-9.1613830879539;-0.265311411396596;-0.0746871460575242]
Robot Arm 1 togoal: 21.7963629462433
Mobile Platform 2 Pose: [-1.5442957412564;9.46922989227089;10.0223522396989]
Robot Arm 2 end: [9.30103625953106;9.61883322410084;0.0709359338748973]
Robot Arm 1 togoal: 27.5595954898232

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-0.354908316787334;-9.20523887035219;-0.119411007477712]
Robot Arm 1 end: [-9.15708481907712;-0.137185824134024;-0.0634766119318252]
Robot Arm 1 togoal: 21.7317049798698
Mobile Platform 2 Pose: [-1.53468275459079;9.46210339279363;10.0036059415872]
Robot Arm 2 end: [9.22250831808138;9.6185748159136;0.0903946204919423]
Robot Arm 1 togoal: 27.503453157707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998741e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-0.368096312454937;-9.19718580153543;0.022751201903483]
Robot Arm 1 end: [-9.12224073001586;-0.00895033142834534;-0.0474361906233389]
Robot Arm 1 togoal: 21.6402211023301
Mobile Platform 2 Pose: [-1.52164131418837;9.45322041421872;9.95892445228023]
Robot Arm 2 end: [9.1691683183209;9.61966151311587;0.0875575192285239]
Robot Arm 1 togoal: 27.4671237891964

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998251e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-0.381282817164554;-9.18912966421485;0.167111724886331]
Robot Arm 1 end: [-9.08902247602957;0.112741467496241;-0.0284705499977333]
Robot Arm 1 togoal: 21.5534108522118
Mobile Platform 2 Pose: [-1.50719096836173;9.44369821577764;9.89576066896593]
Robot Arm 2 end: [9.14869588932038;9.58806015921995;0.0802887877915454]
Robot Arm 1 togoal: 27.430645341659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-0.393919547298524;-9.18133512903422;0.314995612408729]
Robot Arm 1 end: [-9.10740980883158;0.232245135033995;-0.0171208804801651]
Robot Arm 1 togoal: 21.5143663756958
Mobile Platform 2 Pose: [-1.495271179562;9.43551735445662;9.82510792178973]
Robot Arm 2 end: [9.16693853878616;9.56279866672;0.0575978376421982]
Robot Arm 1 togoal: 27.4265595821463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995708e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-0.406053667869945;-9.1738953926995;0.468602156610098]
Robot Arm 1 end: [-9.10765532370542;0.354909704547369;-0.00190357618307496]
Robot Arm 1 togoal: 21.4580869156436
Mobile Platform 2 Pose: [-1.48400160601322;9.42779869144631;9.74905860906405]
Robot Arm 2 end: [9.17313074237072;9.52119097934525;0.0518052704710151]
Robot Arm 1 togoal: 27.4015396076459

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995723e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-0.418009148650038;-9.16658421221993;0.627520871137802]
Robot Arm 1 end: [-9.09634457227068;0.476042313217167;0.0166534201452658]
Robot Arm 1 togoal: 21.3925034690857
Mobile Platform 2 Pose: [-1.47322761863951;9.42044496249227;9.67464271260716]
Robot Arm 2 end: [9.19982160311016;9.45859887281865;0.0490233316956735]
Robot Arm 1 togoal: 27.3758458316186

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-0.428589622359851;-9.15995878836726;0.792591888480511]
Robot Arm 1 end: [-9.09829019543367;0.613417233527814;0.0299193054363905]
Robot Arm 1 togoal: 21.3325091364924
Mobile Platform 2 Pose: [-1.46289961374591;9.41340411764746;9.62038607539312]
Robot Arm 2 end: [9.21876674692615;9.41817401732037;0.0510950201362257]
Robot Arm 1 togoal: 27.3603197731591

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995448e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990476e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-0.439084955872867;-9.1533932134817;0.962182148801232]
Robot Arm 1 end: [-9.08883544875497;0.747117689203782;0.0480735509163272]
Robot Arm 1 togoal: 21.2642653804805
Mobile Platform 2 Pose: [-1.45304211428098;9.40669159025308;9.5854311653567]
Robot Arm 2 end: [9.22788729069322;9.39253146347418;0.0593078099014113]
Robot Arm 1 togoal: 27.3480981109113

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-0.446943032845205;-9.14812398794215;1.13737667930835]
Robot Arm 1 end: [-9.06108695081652;0.87276917614026;0.0919202861566633]
Robot Arm 1 togoal: 21.1819179274947
Mobile Platform 2 Pose: [-1.44368565024942;9.40031254100474;9.566971576207]
Robot Arm 2 end: [9.23253437038704;9.38273916460175;0.0650166445446672]
Robot Arm 1 togoal: 27.344118990424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982881e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948466e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-0.454608417036537;-9.14287607291122;1.3416071167045]
Robot Arm 1 end: [-9.03243186662517;1.05018266433146;0.117271576283807]
Robot Arm 1 togoal: 21.0784996284629
Mobile Platform 2 Pose: [-1.43475248325002;9.39420342294891;9.55814214234296]
Robot Arm 2 end: [9.18855254113222;9.30789378502172;0.0747592034502999]
Robot Arm 1 togoal: 27.2596207508299

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982489e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943980e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-0.462739964853619;-9.13730521014767;1.55322433582578]
Robot Arm 1 end: [-9.0382748289841;1.26655275849503;0.117233505955593]
Robot Arm 1 togoal: 20.9928456754648
Mobile Platform 2 Pose: [-1.41988738371588;9.38489330202011;9.55001837849174]
Robot Arm 2 end: [9.15625352735925;9.27506837416982;0.0740053673182533]
Robot Arm 1 togoal: 27.213674935598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986686e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982781e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-0.470748572456606;-9.13181533860705;1.76835354460614]
Robot Arm 1 end: [-9.03920005366484;1.48598943293651;0.117035316550357]
Robot Arm 1 togoal: 20.9033641180532
Mobile Platform 2 Pose: [-1.40444128010297;9.37513581466327;9.5381667156007]
Robot Arm 2 end: [9.12559452346889;9.24099155091955;0.0762202521511833]
Robot Arm 1 togoal: 27.167839631787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990401e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-0.478732826215294;-9.12633982686125;1.9857950016137]
Robot Arm 1 end: [-9.03645713766772;1.70732740007418;0.116746842051674]
Robot Arm 1 togoal: 20.811704546611
Mobile Platform 2 Pose: [-1.3885146337867;9.36499741806828;9.52216056700227]
Robot Arm 2 end: [9.09667615017137;9.20522895556668;0.0803688131002018]
Robot Arm 1 togoal: 27.1219335121397

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973724e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-0.486769386924633;-9.12082715821546;2.20465621745347]
Robot Arm 1 end: [-9.03156233727558;1.92972485620255;0.116485461220976]
Robot Arm 1 togoal: 20.7196130992797
Mobile Platform 2 Pose: [-1.37221342652895;9.35454973394528;9.50186309696399]
Robot Arm 2 end: [9.06942551497745;9.16760266592172;0.0858085995610171]
Robot Arm 1 togoal: 27.0758149606954

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968021e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-0.49490333606228;-9.11524709063893;2.4243582328948]
Robot Arm 1 end: [-9.0253578749119;2.15256706801683;0.116274326809613]
Robot Arm 1 togoal: 20.6281288561849
Mobile Platform 2 Pose: [-1.35563108977143;9.34385649265001;9.47727324423248]
Robot Arm 2 end: [9.04373034533812;9.12798343517417;0.092027078627349]
Robot Arm 1 togoal: 27.0293429326772

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959229e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-0.503162826142963;-9.10958081326159;2.64450727268593]
Robot Arm 1 end: [-9.01844107218722;2.37553745560114;0.116101343934705]
Robot Arm 1 togoal: 20.5379470770818
Mobile Platform 2 Pose: [-1.33884907805095;9.33297355323175;9.44848992861145]
Robot Arm 2 end: [9.0194980698508;9.08627300317714;0.0986259221345639]
Robot Arm 1 togoal: 26.9824067124905

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000313e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.950395e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-0.511563313794528;-9.10381809781155;2.86486235364472]
Robot Arm 1 end: [-9.01130414854852;2.59849264216289;0.115958372827748]
Robot Arm 1 togoal: 20.449599961144
Mobile Platform 2 Pose: [-1.32193379002022;9.32194739938283;9.41569044652613]
Robot Arm 2 end: [8.99655116368067;9.0424804934376;0.105310823586505]
Robot Arm 1 togoal: 26.9349066325002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000160e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984688e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-0.520065068183348;-9.09797820763578;3.08533681171635]
Robot Arm 1 end: [-9.0173032697673;2.84643505094243;0.115965365696828]
Robot Arm 1 togoal: 20.3667728778586
Mobile Platform 2 Pose: [-1.29742531924731;9.30705956082747;9.37908724351688]
Robot Arm 2 end: [8.97100390651795;8.99663406954655;0.111860719432977]
Robot Arm 1 togoal: 26.8841360044435

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001692e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-0.528732583049871;-9.09203357724106;3.29996315936806]
Robot Arm 1 end: [-8.99511565610189;3.04659924804232;0.119563899057405]
Robot Arm 1 togoal: 20.2762970966357
Mobile Platform 2 Pose: [-1.2732786113639;9.29229742425432;9.3388922965]
Robot Arm 2 end: [8.9466542218999;8.94877830255925;0.118126008504012]
Robot Arm 1 togoal: 26.8328149995447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001506e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980531e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-0.537422808775361;-9.08607072122433;3.51697129736364]
Robot Arm 1 end: [-8.98938450000127;3.27202216703787;0.117181547684162]
Robot Arm 1 togoal: 20.1948784511514
Mobile Platform 2 Pose: [-1.24957243679596;9.2777088217371;9.29535341498377]
Robot Arm 2 end: [8.92348339517537;8.89896420863627;0.124001189811909]
Robot Arm 1 togoal: 26.7809760397649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001585e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-0.546263352285842;-9.08000233324386;3.7320213696432]
Robot Arm 1 end: [-8.96875010704663;3.48696437039903;0.119302598241692]
Robot Arm 1 togoal: 20.1046728235535
Mobile Platform 2 Pose: [-1.22636655099515;9.26333080705147;9.24873577341617]
Robot Arm 2 end: [8.9015180739277;8.84759591645952;0.129713046393309]
Robot Arm 1 togoal: 26.7289157415042

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000581e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970476e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-0.546018789545283;-9.07842665118457;3.94467345531812]
Robot Arm 1 end: [-8.95274464395726;3.70701029925964;0.11918832819863]
Robot Arm 1 togoal: 20.0193237866785
Mobile Platform 2 Pose: [-1.20370519075141;9.24918886413974;9.19923951007392]
Robot Arm 2 end: [8.88069704177883;8.79475623887436;0.135159489359514]
Robot Arm 1 togoal: 26.6766590258802

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999231e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.929216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-0.544720501024021;-9.07732067446204;4.15164485416011]
Robot Arm 1 end: [-8.9318160481882;3.92474979967167;0.118899067029221]
Robot Arm 1 togoal: 19.9320899783556
Mobile Platform 2 Pose: [-1.18161678363018;9.23530045876128;9.14708458934498]
Robot Arm 2 end: [8.8609070025622;8.74055199652039;0.140246956150862]
Robot Arm 1 togoal: 26.6242104307655

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996105e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-0.543493948254055;-9.07610913737408;4.34953406594572]
Robot Arm 1 end: [-8.90824667181458;4.13691623728754;0.118607787052628]
Robot Arm 1 togoal: 19.8460133863128
Mobile Platform 2 Pose: [-0.667434196654309;8.99063526934236;9.09571392566808]
Robot Arm 2 end: [8.61654604837365;8.68638967931125;0.138590042242688]
Robot Arm 1 togoal: 26.4134057499141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000822e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-0.543923008274542;-9.0739979388944;4.53515054422651]
Robot Arm 1 end: [-8.89939994373376;4.3256840669079;0.118353653720887]
Robot Arm 1 togoal: 19.7826337052091
Mobile Platform 2 Pose: [-0.651850281352216;8.97998335439303;9.03971760674859]
Robot Arm 2 end: [8.60331433875222;8.63264286113754;0.138649819852329]
Robot Arm 1 togoal: 26.3660686564698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-0.545991331970823;-9.07103333091251;4.71504250079296]
Robot Arm 1 end: [-8.88861095312872;4.50927297381891;0.118222491403572]
Robot Arm 1 togoal: 19.7204333561236
Mobile Platform 2 Pose: [-0.636246971774757;8.9693183371722;8.98317009387763]
Robot Arm 2 end: [8.59008827598742;8.5783601773884;0.138708213299655]
Robot Arm 1 togoal: 26.3183886203801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998508e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-0.550931465539834;-9.0666007474918;4.88769928837932]
Robot Arm 1 end: [-8.87451715367151;4.68639906218017;0.118193404009321]
Robot Arm 1 togoal: 19.6583218779324
Mobile Platform 2 Pose: [-0.620624636515101;8.9586404702191;8.92607227184689]
Robot Arm 2 end: [8.57686820147563;8.5235422794913;0.138765247097247]
Robot Arm 1 togoal: 26.2703677870568

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999878e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-0.559891964521497;-9.06012742700123;5.05094852330733]
Robot Arm 1 end: [-8.85662995125993;4.85510543992708;0.11829933199488]
Robot Arm 1 togoal: 19.5961731549056
Mobile Platform 2 Pose: [-0.604983642247691;8.94795000460836;8.86842503166798]
Robot Arm 2 end: [8.56365444707041;8.46818982614594;0.138820947874928]
Robot Arm 1 togoal: 26.2220083292057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-0.573852661390452;-9.05112771993521;5.20254771597186]
Robot Arm 1 end: [-8.83461966367552;5.01336684470748;0.118575769380071]
Robot Arm 1 togoal: 19.5339755765027
Mobile Platform 2 Pose: [-0.589324353149388;8.93724718971626;8.81022926556123]
Robot Arm 2 end: [8.5504473412204;8.4123034759415;0.138875342596119]
Robot Arm 1 togoal: 26.1733124464245

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-0.593422138610879;-9.03930446742603;5.34033950510362]
Robot Arm 1 end: [-8.80839967844511;5.15922632924088;0.119059278793806]
Robot Arm 1 togoal: 19.4718880949287
Mobile Platform 2 Pose: [-0.573647131206737;8.92653227341505;8.7514858632832]
Robot Arm 2 end: [8.53724720394468;8.35588388527805;0.138928460283376]
Robot Arm 1 togoal: 26.1242823604948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-0.618669648967308;-9.02463262682649;5.46243080255908]
Robot Arm 1 end: [-8.77819171330469;5.29096088250279;0.119785980807434]
Robot Arm 1 togoal: 19.4102790850448
Mobile Platform 2 Pose: [-0.557952335958001;8.91580550179079;8.69219570730247]
Robot Arm 2 end: [8.52405435140514;8.29893170205845;0.138980330451723]
Robot Arm 1 togoal: 26.0749203146899

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-0.649057107423722;-9.00739246251298;5.56737679291855]
Robot Arm 1 end: [-8.74454244939696;5.40725874233451;0.120789994968476]
Robot Arm 1 togoal: 19.3497217539503
Mobile Platform 2 Pose: [-0.54224032486453;8.90506711965122;8.63235966887387]
Robot Arm 2 end: [8.51086909444788;8.24144756242189;0.139030983734895]
Robot Arm 1 togoal: 26.025228570875

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-0.68351044285622;-8.98813262982587;5.65434550957422]
Robot Arm 1 end: [-8.7082734355926;5.50738638366637;0.122101319253145]
Robot Arm 1 togoal: 19.2909302410631
Mobile Platform 2 Pose: [-0.526511453638237;8.89431737043891;8.57197860383648]
Robot Arm 2 end: [8.49769173572878;8.18343208814511;0.139080452511692]
Robot Arm 1 togoal: 25.975209406085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-0.715469577448604;-8.970143746245;5.72323797703168]
Robot Arm 1 end: [-8.70953935651604;5.5785450423791;0.114455668478084]
Robot Arm 1 togoal: 19.2762676322941
Mobile Platform 2 Pose: [-0.510766075779791;8.88355649608359;8.51105334797877]
Robot Arm 2 end: [8.48452257573823;8.12488587860457;0.139128769550918]
Robot Arm 1 togoal: 25.9248651117602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-0.755066790580866;-8.94832031542241;5.78859030333009]
Robot Arm 1 end: [-8.64797664683939;5.65999891783907;0.123745851127833]
Robot Arm 1 togoal: 19.1972644293599
Mobile Platform 2 Pose: [-0.495004543247534;8.87278473746011;8.44958471365296]
Robot Arm 2 end: [8.4713619076947;8.06580951131114;0.139175968867328]
Robot Arm 1 togoal: 25.8741979909915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-0.770803610418438;-8.93847658284424;5.837015561597]
Robot Arm 1 end: [-8.65703582954354;5.7088909139915;0.116007282777445]
Robot Arm 1 togoal: 19.1956403297297
Mobile Platform 2 Pose: [-0.479227206207731;8.86200233417871;8.38757348520852]
Robot Arm 2 end: [8.4582100205472;8.00620353510949;0.139222085243258]
Robot Arm 1 togoal: 25.8232103565015

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-0.781611911150604;-8.93108830046601;5.88236329621338]
Robot Arm 1 end: [-8.61010096231659;5.77504923258595;0.126448518881726]
Robot Arm 1 togoal: 19.134545739269
Mobile Platform 2 Pose: [-0.463434413276083;8.85120952476238;8.32502041526899]
Robot Arm 2 end: [8.44506719784295;7.9460684665516;0.139267154847853]
Robot Arm 1 togoal: 25.771904527886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-0.79219296508684;-8.92386968953391;5.91179052512657]
Robot Arm 1 end: [-8.59513627697322;5.81294480788729;0.127435289660751]
Robot Arm 1 togoal: 19.1115814872267
Mobile Platform 2 Pose: [-0.44762651181503;8.84040654681823;8.26192622053308]
Robot Arm 2 end: [8.4319337177938;7.88540478684395;0.139311214771457]
Robot Arm 1 togoal: 25.7202828302029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-0.802646295485012;-8.91673112925873;5.93517358141442]
Robot Arm 1 end: [-8.57017739686204;5.85858534932848;0.13146274913565]
Robot Arm 1 togoal: 19.0770442582509
Mobile Platform 2 Pose: [-0.431803847297723;8.82959363673142;8.19829157794431]
Robot Arm 2 end: [8.41880985559733;7.82421293623387;0.139354302537584]
Robot Arm 1 togoal: 25.6683475924371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000140e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-0.81290876761835;-8.90973365081248;5.94539382068503]
Robot Arm 1 end: [-8.5804947776683;5.88069678019458;0.121547929402414]
Robot Arm 1 togoal: 19.0830267855379
Mobile Platform 2 Pose: [-0.415966764720206;8.8187710304491;8.13411712120246]
Robot Arm 2 end: [8.40569588104283;7.76249331261006;0.139396456758747]
Robot Arm 1 togoal: 25.6161011455032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-0.823046078006449;-8.90282530747992;5.95087247148083]
Robot Arm 1 end: [-8.54255266354665;5.90646975975666;0.133407521279753]
Robot Arm 1 togoal: 19.0396602384388
Mobile Platform 2 Pose: [-0.400115607653769;8.8079389629271;8.06940343672714]
Robot Arm 2 end: [8.39259205979018;7.70024626611232;0.139437716842034]
Robot Arm 1 togoal: 25.5635458201933

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002126e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-0.832933002642903;-8.89603941195745;5.95321399288588]
Robot Arm 1 end: [-8.51796957567209;5.97162404494045;0.140776441569555]
Robot Arm 1 togoal: 19.0012638293369
Mobile Platform 2 Pose: [-0.384250718892044;8.79709766857471;8.00415106014356]
Robot Arm 2 end: [8.37949865329812;7.63747209624594;0.139478122978145]
Robot Arm 1 togoal: 25.51068394593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001872e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-0.842193197193357;-8.88960787659847;5.94888046026988]
Robot Arm 1 end: [-8.52055768165683;6.03793346661681;0.140428019473511]
Robot Arm 1 togoal: 18.9898659038959
Mobile Platform 2 Pose: [-0.36837244055783;8.78624738127444;7.93836047266546]
Robot Arm 2 end: [8.3664159197737;7.57417104778354;0.139517715885168]
Robot Arm 1 togoal: 25.4575178494722

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001647e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-0.851261334268305;-8.88330200739632;5.94683830468512]
Robot Arm 1 end: [-8.53631330908896;6.10378611320226;0.140132926828869]
Robot Arm 1 togoal: 18.9916346747233
Mobile Platform 2 Pose: [-0.352481113922888;8.77538833441572;7.87203209745654]
Robot Arm 2 end: [8.3533441114008;7.51034330857095;0.139556537632216]
Robot Arm 1 togoal: 25.4040498522426

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001376e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-0.860077565549904;-8.87716107310979;5.95193158325596]
Robot Arm 1 end: [-8.56458752401255;6.17164776202135;0.139878095953877]
Robot Arm 1 togoal: 19.0054647012009
Mobile Platform 2 Pose: [-0.336577080268293;8.76452076127349;7.8051662959661]
Robot Arm 2 end: [8.34028347995022;7.4459890032417;0.139594630088451]
Robot Arm 1 togoal: 25.3502822710554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000190e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-0.869450674946228;-8.87078893884634;5.96876356578908]
Robot Arm 1 end: [-8.57582150832206;6.18421242073363;0.130302992327066]
Robot Arm 1 togoal: 19.0146106383032
Mobile Platform 2 Pose: [-0.320660680349409;8.7536448947531;7.73776336503892]
Robot Arm 2 end: [8.32723427030067;7.38110819381603;0.139632036646153]
Robot Arm 1 togoal: 25.2962174147401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-0.878645087983073;-8.86454645324216;5.9907483082433]
Robot Arm 1 end: [-8.57125319693201;6.20211035794717;0.127584919196925]
Robot Arm 1 togoal: 19.0067623919798
Mobile Platform 2 Pose: [-0.304732254833325;8.74276096767851;7.66982353274475]
Robot Arm 2 end: [8.3141967266382;7.31570087211081;0.139668800621485]
Robot Arm 1 togoal: 25.2418575845123

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000007e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-0.887787166246665;-8.85833964959534;6.0134628094042]
Robot Arm 1 end: [-8.56511594037754;6.22462764647006;0.12662355315069]
Robot Arm 1 togoal: 18.9963483849656
Mobile Platform 2 Pose: [-0.288792144401162;8.73186921284942;7.6013469558325]
Robot Arm 2 end: [8.3011710892933;7.24976695910545;0.139704966106132]
Robot Arm 1 togoal: 25.187205072292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001726e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-0.896892015957092;-8.85215559482648;6.03662617277907]
Robot Arm 1 end: [-8.56173622165353;6.25305239916575;0.126156363489876]
Robot Arm 1 togoal: 18.9874499777078
Mobile Platform 2 Pose: [-0.27284069023799;8.72096986341459;7.53233371630708]
Robot Arm 2 end: [8.28815759561539;7.18330630147892;0.139740577740854]
Robot Arm 1 togoal: 25.1322621601019

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001421e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-0.905997989137051;-8.84597769457737;6.06142680307881]
Robot Arm 1 end: [-8.54987973786602;6.27206587552219;0.125856411637411]
Robot Arm 1 togoal: 18.9721363256194
Mobile Platform 2 Pose: [-0.256878233701356;8.71006315259021;7.46278381812322]
Robot Arm 2 end: [8.27515648073363;7.11631866762364;0.139775680445933]
Robot Arm 1 togoal: 25.0770311190912

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002597e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-0.915150264250349;-8.83977203358214;6.08453403699372]
Robot Arm 1 end: [-8.5245675247754;6.30064537386007;0.134543038575857]
Robot Arm 1 togoal: 18.9411480522113
Mobile Platform 2 Pose: [-0.240905116902426;8.69914931409546;7.39269718419575]
Robot Arm 2 end: [8.26216797637912;7.04880374533593;0.139810319982657]
Robot Arm 1 togoal: 25.0215142083099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002234e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-0.925118135764839;-8.83311886987712;6.10282323194831]
Robot Arm 1 end: [-8.47744748598197;6.2652272642362;0.138443813044877]
Robot Arm 1 togoal: 18.9017473952837
Mobile Platform 2 Pose: [-0.224921682447588;8.68822858193043;7.322073653288]
Robot Arm 2 end: [8.24919231116785;6.9807611390429;0.13984454258164]
Robot Arm 1 togoal: 24.9657136743329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970238e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-0.936172901051184;-8.82585565377886;6.10881124935343]
Robot Arm 1 end: [-8.44535881637576;6.20405731825541;0.138233072488051]
Robot Arm 1 togoal: 18.88259746655
Mobile Platform 2 Pose: [-0.20892827438265;8.67730119107186;7.25091297694538]
Robot Arm 2 end: [8.23622971427294;6.91219036414193;0.139878394373758]
Robot Arm 1 togoal: 24.9096317514779

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949818e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-0.947805059920526;-8.8182528627861;6.10738067608632]
Robot Arm 1 end: [-8.42558209282702;6.13187218061673;0.137992640967552]
Robot Arm 1 togoal: 18.877955335304
Mobile Platform 2 Pose: [-0.192925237346777;8.66636737694474;7.17921481700534]
Robot Arm 2 end: [8.22328040998045;6.84309084923353;0.139911922108484]
Robot Arm 1 togoal: 24.853270660724

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-0.959773990313579;-8.81043685053193;6.10313852199505]
Robot Arm 1 end: [-8.41873358372557;6.05567099694783;0.137709613649485]
Robot Arm 1 togoal: 18.8870582782377
Mobile Platform 2 Pose: [-0.176912917893517;8.6554273761451;7.10697874235213]
Robot Arm 2 end: [8.21034462351104;6.77346192750721;0.139945172589633]
Robot Arm 1 togoal: 24.7966326096284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-0.970202004660514;-8.80335408865805;6.10081445420596]
Robot Arm 1 end: [-8.45222645161668;6.01868796850893;0.118439896875639]
Robot Arm 1 togoal: 18.9288848418684
Mobile Platform 2 Pose: [-0.160891663547073;8.64448142602148;7.03420422649618]
Robot Arm 2 end: [8.19742257559917;6.7033028388496;0.139978193544656]
Robot Arm 1 togoal: 24.739719791282

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-0.980258271183081;-8.79651389110507;6.0985159329819]
Robot Arm 1 end: [-8.43905739337357;6.01910647880688;0.120742520332312]
Robot Arm 1 togoal: 18.9157889550652
Mobile Platform 2 Pose: [-0.144861824044794;8.6335297653373;6.96089064449584]
Robot Arm 2 end: [8.18451448852918;6.63261272325975;0.140011032094783]
Robot Arm 1 togoal: 24.6825343859772

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-0.990381184010549;-8.78962818230271;6.09642216787368]
Robot Arm 1 end: [-8.42958061491475;6.01843514062156;0.121089406245359]
Robot Arm 1 togoal: 18.906666814687
Mobile Platform 2 Pose: [-0.128823750797986;8.62257263398514;6.88703727069502]
Robot Arm 2 end: [8.17162058118482;6.56139062134873;0.14004373619948]
Robot Arm 1 togoal: 24.6250785595085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-1.0005268689041;-8.78272696030515;6.09441520409632]
Robot Arm 1 end: [-8.42110629694722;6.0174960357968;0.120886435552871]
Robot Arm 1 togoal: 18.8986193416787
Mobile Platform 2 Pose: [-0.112777797408467;8.61161027334174;6.81264327580929]
Robot Arm 2 end: [8.15874107215776;6.48963546983078;0.140076353775044]
Robot Arm 1 togoal: 24.5673544642127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-1.01013964261555;-8.776090313738;6.09246355456847]
Robot Arm 1 end: [-8.44395050579491;6.00696863611898;0.10799006684496]
Robot Arm 1 togoal: 18.9240642253281
Mobile Platform 2 Pose: [-0.096724319733868;8.60064292629326;6.73770772454515]
Robot Arm 2 end: [8.14587617915105;6.41734609952619;0.140108932871536]
Robot Arm 1 togoal: 24.5093642390003

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987085e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-1.00953238955334;-8.77461478609947;6.08939619215795]
Robot Arm 1 end: [-8.37898089949074;6.02411164523598;0.1358097167781]
Robot Arm 1 togoal: 18.8550624298655
Mobile Platform 2 Pose: [-0.0806636764917452;8.58967083769461;6.66222957305034]
Robot Arm 2 end: [8.13302611939123;6.34452123244938;0.140141521826495]
Robot Arm 1 togoal: 24.4511100095938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992952e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-1.01227556568564;-8.77135719303353;6.08886143869606]
Robot Arm 1 end: [-8.37448645985858;6.02468253793115;0.135489919927933]
Robot Arm 1 togoal: 18.8505846002117
Mobile Platform 2 Pose: [-0.0645962287095657;8.57869425390329;6.58620766649958]
Robot Arm 2 end: [8.12019110909191;6.27115948103999;0.140174168662383]
Robot Arm 1 togoal: 24.3925938896203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996459e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-1.01653593358999;-8.76734038806209;6.08830928380733]
Robot Arm 1 end: [-8.34833338945159;6.05479308608154;0.13422100281377]
Robot Arm 1 togoal: 18.8188509678761
Mobile Platform 2 Pose: [-0.0485223406601383;8.56771342350916;6.5096407368934]
Robot Arm 2 end: [8.10737136507007;6.19725934322793;0.140206921644342]
Robot Arm 1 togoal: 24.3338179805303

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998970e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-1.02352103470293;-8.76195174108394;6.08616271099709]
Robot Arm 1 end: [-8.35526683374196;6.0520135564714;0.125733661736498]
Robot Arm 1 togoal: 18.8268209119555
Mobile Platform 2 Pose: [-0.0324423797340068;8.55672859720158;6.43252740053169]
Robot Arm 2 end: [8.09456710388691;6.12281920158156;0.140239828990672]
Robot Arm 1 togoal: 24.27478437255

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-1.03277570918251;-8.75545559720151;6.08341112955596]
Robot Arm 1 end: [-8.31456310614041;6.09467403501688;0.135402283692418]
Robot Arm 1 togoal: 18.777504458607
Mobile Platform 2 Pose: [-0.0163567167899996;8.54574002802785;6.35486615606473]
Robot Arm 2 end: [8.08177854231101;6.04783732105161;0.140272938879526]
Robot Arm 1 togoal: 24.2154951457414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999761e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-1.04518208212716;-8.74737512070206;6.08092708344581]
Robot Arm 1 end: [-8.34194811702277;6.07692053182863;0.113219916711179]
Robot Arm 1 togoal: 18.8095576238264
Mobile Platform 2 Pose: [-0.000265726109214014;8.53474797132669;6.27665538217819]
Robot Arm 2 end: [8.06900589767467;5.97231184652517;0.140306299255413]
Robot Arm 1 togoal: 24.1559523709608

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-1.06201046762811;-8.73716339993243;6.07803309662564]
Robot Arm 1 end: [-8.30613590731904;6.08603868639337;0.128416103437386]
Robot Arm 1 togoal: 18.771600755679
Mobile Platform 2 Pose: [0.0158302138006349;8.5237526853029;6.19789333535597]
Robot Arm 2 end: [8.05624938634101;5.8962408018006;0.140339958558375]
Robot Arm 1 togoal: 24.0961581103836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-1.08302377887037;-8.72480368380595;6.07568287267626]
Robot Arm 1 end: [-8.28499019581641;6.08812862466198;0.133859396850393]
Robot Arm 1 togoal: 18.7501403507066
Mobile Platform 2 Pose: [0.0319307209020466;8.51275443062091;6.11857814761271]
Robot Arm 2 end: [8.04350922754052;5.81962208461363;0.140373964284064]
Robot Arm 1 togoal: 24.0361144196568

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991974e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-1.11006910025321;-8.70940961851612;6.07366916888753]
Robot Arm 1 end: [-8.26220876002316;6.08800660391837;0.132526585920504]
Robot Arm 1 togoal: 18.7280490053625
Mobile Platform 2 Pose: [0.0480354082527957;8.5017534711769;6.03870782483232]
Robot Arm 2 end: [8.03078563962247;5.74245346775273;0.140408364511295]
Robot Arm 1 togoal: 23.9758233483306

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971425e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-1.08817339366341;-8.71847568623076;6.07170759469613]
Robot Arm 1 end: [-8.27021010935675;6.08731767233023;0.132483203138434]
Robot Arm 1 togoal: 18.7359984877366
Mobile Platform 2 Pose: [0.064143884974367;8.49075007363861;5.95828024463646]
Robot Arm 2 end: [8.0180788439091;5.66473259439719;0.140443206361538]
Robot Arm 1 togoal: 23.9152869424508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-1.06764684524259;-8.72685777503073;6.06968175920628]
Robot Arm 1 end: [-8.27753130769736;6.0865532815288;0.132445877517204]
Robot Arm 1 togoal: 18.7433000955871
Mobile Platform 2 Pose: [0.080255755016972;8.47974450809937;5.87729315439341]
Robot Arm 2 end: [8.00538906137721;5.58645697866264;0.140478537343361]
Robot Arm 1 togoal: 23.8545072451333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-1.05080346136297;-8.73339883373051;6.06760011490623]
Robot Arm 1 end: [-8.28301446598207;6.08570363037222;0.132414574049105]
Robot Arm 1 togoal: 18.7488267237647
Mobile Platform 2 Pose: [0.0963706173023324;8.46873704812095;5.79574416925477]
Robot Arm 2 end: [7.99271651551217;5.50762400154076;0.140514404435542]
Robot Arm 1 togoal: 23.7934862989918

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992381e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-1.04043834617012;-8.73670119231845;6.065483507445]
Robot Arm 1 end: [-8.28526066010586;6.08476743052084;0.132389253957547]
Robot Arm 1 togoal: 18.7512144482737
Mobile Platform 2 Pose: [0.112488065681176;8.45772797068735;5.7136307703942]
Robot Arm 2 end: [7.98006143128136;5.42823091002506;0.140550854342678]
Robot Arm 1 togoal: 23.7322261477816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-1.04118809268886;-8.73444633369516;6.06336758898681]
Robot Arm 1 end: [-8.30035877640195;6.07578737782228;0.120842128439093]
Robot Arm 1 togoal: 18.7686711866979
Mobile Platform 2 Pose: [0.128607688371756;8.44671755662806;5.63095030317579]
Robot Arm 2 end: [7.96742403511518;5.34827481553612;0.140587933641785]
Robot Arm 1 togoal: 23.6707288384579

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-1.05142317745803;-8.72748845567495;6.06102999504318]
Robot Arm 1 end: [-8.30138843490105;6.07025595201393;0.114784288220339]
Robot Arm 1 togoal: 18.7712852398515
Mobile Platform 2 Pose: [0.144729068076121;8.43570609049698;5.54769997527032]
Robot Arm 2 end: [7.95480455725603;5.26775269020558;0.140625687931147]
Robot Arm 1 togoal: 23.6089964238203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-1.06155257714935;-8.72060290472352;6.05866360564298]
Robot Arm 1 end: [-8.29593189483875;6.06767296626611;0.11308678789872]
Robot Arm 1 togoal: 18.7666336729212
Mobile Platform 2 Pose: [0.160851781268965;8.42469386111168;5.46387685539734]
Robot Arm 2 end: [7.94220322879543;5.18666136811326;0.140664163000336]
Robot Arm 1 togoal: 23.5470309643498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-1.07165562542299;-8.71373541850657;6.05638226247622]
Robot Arm 1 end: [-8.28869175502757;6.06599716045136;0.112563157091129]
Robot Arm 1 togoal: 18.7599658526462
Mobile Platform 2 Pose: [0.176975398244055;8.41368116148307;5.37947787116654]
Robot Arm 2 end: [7.92962028370312;5.10499754178853;0.140703404067874]
Robot Arm 1 togoal: 23.4848345309895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-1.08175410898071;-8.70687109678048;6.05421517226374]
Robot Arm 1 end: [-8.28096405970038;6.06466038336856;0.112364237133824]
Robot Arm 1 togoal: 18.7527277899166
Mobile Platform 2 Pose: [0.19309948264993;8.40266828912064;5.29449980740902]
Robot Arm 2 end: [7.91705595821331;5.02275776108506;0.140743456101739]
Robot Arm 1 togoal: 23.4224092075295

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-1.09185396700402;-8.70000587774331;6.0521701724707]
Robot Arm 1 end: [-8.27310307509812;6.06350671637607;0.11225855267595]
Robot Arm 1 togoal: 18.7453148234588
Mobile Platform 2 Pose: [0.209223591866193;8.39165554586683;5.20893930469335]
Robot Arm 2 end: [7.90451049344212;4.9399384293231;0.140784362876494]
Robot Arm 1 togoal: 23.3597570939148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-1.10195679977059;-8.69313866689079;6.05024938022005]
Robot Arm 1 end: [-8.26520542131593;6.06249350601564;0.11218259263897]
Robot Arm 1 togoal: 18.7378347991523
Mobile Platform 2 Pose: [0.225347275438541;8.38064323882433;5.12279285806207]
Robot Arm 2 end: [7.89198413129771;4.856535806364;0.140826168594751]
Robot Arm 1 togoal: 23.2968803088585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-1.11206302123333;-8.68626918098938;6.04845335640823]
Robot Arm 1 end: [-8.2572974470021;6.06160906719039;0.112118892051482]
Robot Arm 1 togoal: 18.7303171138206
Mobile Platform 2 Pose: [0.241470076383754;8.36963167961035;5.03605681491454]
Robot Arm 2 end: [7.87947711927825;4.77254600124567;0.1408689161695]
Robot Arm 1 togoal: 23.2337809929049

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001815e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-1.12214061834296;-8.67941342521575;6.0467822542514]
Robot Arm 1 end: [-8.21949247146206;6.08983002362675;0.132565502589603]
Robot Arm 1 togoal: 18.6860130984486
Mobile Platform 2 Pose: [0.257591529705325;8.35862118522925;4.94872737418473]
Robot Arm 2 end: [7.86698970705417;4.68796497627925;0.140912648242392]
Robot Arm 1 togoal: 23.1704613125174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003199e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-1.13204824240248;-8.67259324230742;6.04583404211436]
Robot Arm 1 end: [-8.22770229974275;6.17420607345136;0.132607792390041]
Robot Arm 1 togoal: 18.6765484824851
Mobile Platform 2 Pose: [0.273711162540822;8.34761207808251;4.86080058443229]
Robot Arm 2 end: [7.85452214889995;4.60278854220759;0.140957406615236]
Robot Arm 1 togoal: 23.1069234631862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002728e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-1.14125055869359;-8.66624756641698;6.04871375900132]
Robot Arm 1 end: [-8.25203551881595;6.25828082911828;0.132708301592258]
Robot Arm 1 togoal: 18.6832710405253
Mobile Platform 2 Pose: [0.289828493884746;8.33660468611679;4.7722723427866]
Robot Arm 2 end: [7.84207470268303;4.51701235736345;0.141003232861062]
Robot Arm 1 togoal: 23.0431696725359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002200e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-1.14969821300122;-8.66040836118832;6.06021443071014]
Robot Arm 1 end: [-8.29152793031293;6.34401369095825;0.13285467414403]
Robot Arm 1 togoal: 18.7048971492231
Mobile Platform 2 Pose: [0.305943034408007;8.32559934293553;4.68313839318442]
Robot Arm 2 end: [7.82964763069846;4.43063192641358;0.141050167687027]
Robot Arm 1 togoal: 22.9792022048242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001614e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-1.15736377118941;-8.65509284498168;6.08484359850277]
Robot Arm 1 end: [-8.34469540286163;6.43329781541814;0.133033192025371]
Robot Arm 1 togoal: 18.7396815810011
Mobile Platform 2 Pose: [0.322054285975347;8.3145963881502;4.59339432522152]
Robot Arm 2 end: [7.81724119911925;4.34364259962492;0.141098251347649]
Robot Arm 1 togoal: 22.9150233647994

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-1.16494487723573;-8.64995110022956;6.12667843955001]
Robot Arm 1 end: [-8.33890102816805;6.47621912012799;0.133229379102404]
Robot Arm 1 togoal: 18.7258706047682
Mobile Platform 2 Pose: [0.338161741337339;8.30359616755681;4.50303557273031]
Robot Arm 2 end: [7.80485568127487;4.25603956815187;0.141147522661863]
Robot Arm 1 togoal: 22.8506355022996

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001485e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-1.17249241297685;-8.64482921041701;6.16895752736131]
Robot Arm 1 end: [-8.33922121610298;6.52490630782334;0.133430249714]
Robot Arm 1 togoal: 18.7170687019756
Mobile Platform 2 Pose: [0.354264884200448;8.29259903318509;4.41205741310624]
Robot Arm 2 end: [7.79249135226005;4.16781786982954;0.141198020418811]
Robot Arm 1 togoal: 22.7860410167226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001943e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-1.17995947673852;-8.63976263212321;6.21344690109836]
Robot Arm 1 end: [-8.33869978698331;6.57486506535491;0.133636254911655]
Robot Arm 1 togoal: 18.7073311126631
Mobile Platform 2 Pose: [0.370363188151297;8.28160534387332;4.32045496541584]
Robot Arm 2 end: [7.78014849360003;4.07897238133643;0.14124978263264]
Robot Arm 1 togoal: 22.7212423610864

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001311e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-1.18737391371742;-8.6347357799259;6.25991118475709]
Robot Arm 1 end: [-8.33023782793999;6.62006409059912;0.133847223236149]
Robot Arm 1 togoal: 18.6907939479997
Mobile Platform 2 Pose: [0.386456117330082;8.27061546498422;4.22822318938024]
Robot Arm 2 end: [7.76782739300402;3.98949781845576;0.141302846035013]
Robot Arm 1 togoal: 22.6562420475423

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-1.19479359776963;-8.62970452479621;6.30604540822213]
Robot Arm 1 end: [-8.32357028309685;6.66643275162893;0.134062698483747]
Robot Arm 1 togoal: 18.6759067677406
Mobile Platform 2 Pose: [0.40254312517871;8.25962976910859;4.13535688454341]
Robot Arm 2 end: [7.75552834336378;3.89938873648908;0.141357246768032]
Robot Arm 1 togoal: 22.5910426524891

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-1.20220151948981;-8.62468081971222;6.35237130720939]
Robot Arm 1 end: [-8.31791229989438;6.71383946450797;0.13428274095974]
Robot Arm 1 togoal: 18.6619330951584
Mobile Platform 2 Pose: [0.418623654640357;8.24864863601641;4.04185068908418]
Robot Arm 2 end: [7.74325164340743;3.80863952835986;0.141413020116802]
Robot Arm 1 togoal: 22.5256468218013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-1.20958920978618;-8.61967091983897;6.39918475534882]
Robot Arm 1 end: [-8.31226750318461;6.76172999004938;0.134507387175162]
Robot Arm 1 togoal: 18.6479997729235
Mobile Platform 2 Pose: [0.434697137706679;8.23767245294415;3.94769907882164]
Robot Arm 2 end: [7.73099759853999;3.7172444230717;0.141470200283881]
Robot Arm 1 togoal: 22.4600572766417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-1.21695623772761;-8.61467511885715;6.44649018415553]
Robot Arm 1 end: [-8.30663616367192;6.81010869838092;0.134736615078101]
Robot Arm 1 togoal: 18.6341101294798
Mobile Platform 2 Pose: [0.450762994919778;8.22670161491255;3.85289636620757]
Robot Arm 2 end: [7.71876651965825;3.62519748595248;0.141528820916757]
Robot Arm 1 togoal: 22.3942768189395

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-1.22430216632478;-8.6096937145727;6.49429205431094]
Robot Arm 1 end: [-8.30101855584818;6.85897998836005;0.13497040217624]
Robot Arm 1 togoal: 18.6202675851091
Mobile Platform 2 Pose: [0.466820635395435;8.21573652477964;3.75743669932953]
Robot Arm 2 end: [7.70655872438212;3.53249261680055;0.141588914655498]
Robot Arm 1 togoal: 22.328308337761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-1.23162655247844;-8.60472700895196;6.54259485527097]
Robot Arm 1 end: [-8.29541495803212;6.90834828717851;0.135208725530755]
Robot Arm 1 togoal: 18.6064756538247
Mobile Platform 2 Pose: [0.482869456263224;8.20477759357327;3.66131406092189]
Robot Arm 2 end: [7.69437453756131;3.43912354831125;0.141650513047149]
Robot Arm 1 togoal: 22.2621548155031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-1.23892894692871;-8.59977530815604;6.59140310484452]
Robot Arm 1 end: [-8.28982565240831;6.95821804993736;0.135451561750095]
Robot Arm 1 togoal: 18.5927379452827
Mobile Platform 2 Pose: [0.498908841900864;8.1938252409547;3.56452226769251]
Robot Arm 2 end: [7.68221429085661;3.34508384635736;0.14171364684451]
Robot Arm 1 togoal: 22.1958193346874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-1.24620889420542;-8.59483892257448;6.6407213487415]
Robot Arm 1 end: [-8.28425092506565;7.00859375919151;0.135698886983637]
Robot Arm 1 togoal: 18.5790581667112
Mobile Platform 2 Pose: [0.514938164274351;8.18287989511497;3.46705496952424]
Robot Arm 2 end: [7.6700783242929;3.25036690728111;0.141778345478661]
Robot Arm 1 togoal: 22.1293050848082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-1.25346593257953;-8.5899181668582;6.69055416008898]
Robot Arm 1 end: [-8.27869106603544;7.05947992446272;0.135950676915237]
Robot Arm 1 togoal: 18.5654401248551
Mobile Platform 2 Pose: [0.530956782149067;8.171941993254;3.36890564888307]
Robot Arm 2 end: [7.65796698486296;3.15496596045266;0.141844637297017]
Robot Arm 1 togoal: 22.062615370206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-1.26069959401595;-8.58501335995149;6.7409061389146]
Robot Arm 1 end: [-8.27314636932881;7.11088108171982;0.136206906756653]
Robot Arm 1 togoal: 18.5518877279368
Mobile Platform 2 Pose: [0.546964040587627;8.16101198183454;3.27006762021186]
Robot Arm 2 end: [7.64588062685512;3.0588740652045;0.141912550172185]
Robot Arm 1 togoal: 21.9957536161497

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-1.25959310372794;-8.5842829760472;6.79178191176994]
Robot Arm 1 end: [-8.27665440954808;7.15104161770465;0.128371723463823]
Robot Arm 1 togoal: 18.5497189678634
Mobile Platform 2 Pose: [0.562959271003047;8.15009031673429;3.17053402892968]
Robot Arm 2 end: [7.63381961438831;2.96208410911943;0.141982110143607]
Robot Arm 1 togoal: 21.9287233779173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-1.25485119461423;-8.58540722855302;6.84470089378917]
Robot Arm 1 end: [-8.27755095712809;7.2046154047905;0.128932381284751]
Robot Arm 1 togoal: 18.5424080496742
Mobile Platform 2 Pose: [0.578941790134235;8.13917746377603;3.07029785149155]
Robot Arm 2 end: [7.62178431853806;2.86458881071683;0.14205334277672]
Robot Arm 1 togoal: 21.8615283482505

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-1.24671667907883;-8.58824423865797;6.89848106510987]
Robot Arm 1 end: [-8.27905013732855;7.26188564261265;0.13153819404448]
Robot Arm 1 togoal: 18.5351435998567
Mobile Platform 2 Pose: [0.594910900117017;8.12827389872734;2.96935189458597]
Robot Arm 2 end: [7.60977512077663;2.76638071554028;0.142126271852105]
Robot Arm 1 togoal: 21.7941723664061

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-1.23514381292486;-8.59281143291824;6.95279871801849]
Robot Arm 1 end: [-8.28205479389536;7.32027081912884;0.134577700592294]
Robot Arm 1 togoal: 18.5293454488306
Mobile Platform 2 Pose: [0.610865887946516;8.11738010774162;2.86768879513831]
Robot Arm 2 end: [7.59779240977567;2.66745219948856;0.142200920720705]
Robot Arm 1 togoal: 21.7266594262649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-1.22014156566882;-8.59910288508535;7.00755488180232]
Robot Arm 1 end: [-8.28678816357737;7.37909297487876;0.137631082637664]
Robot Arm 1 togoal: 18.5253740727524
Mobile Platform 2 Pose: [0.626806024996676;8.10649658763149;2.7653010195876]
Robot Arm 2 end: [7.58583658514403;2.56779546461345;0.142277310951539]
Robot Arm 1 togoal: 21.658993686066

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-1.20183984018835;-8.6070532643781;7.0627101181031]
Robot Arm 1 end: [-8.29336269712795;7.43782482809063;0.140253770180744]
Robot Arm 1 togoal: 18.5234532561124
Mobile Platform 2 Pose: [0.64273056639062;8.09562384621539;2.66218086411347]
Robot Arm 2 end: [7.57390805561419;2.46740254149837;0.14235546314555]
Robot Arm 1 togoal: 21.5911794776927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-1.17870549128127;-8.61742861600851;7.11824556007425]
Robot Arm 1 end: [-8.30175027450838;7.49599501158268;0.138149945627631]
Robot Arm 1 togoal: 18.5239406813281
Mobile Platform 2 Pose: [0.6586387508776;8.08476240256246;2.55832045440431]
Robot Arm 2 end: [7.56200724014466;2.36626528792145;0.142435396559104]
Robot Arm 1 togoal: 21.5232213165224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-1.15507729800671;-8.62799632208283;7.17219385606179]
Robot Arm 1 end: [-8.31187905896061;7.55112428625196;0.138439119388235]
Robot Arm 1 togoal: 18.5265588148469
Mobile Platform 2 Pose: [0.674529800250853;8.07391278730382;2.45371174596266]
Robot Arm 2 end: [7.55013456848804;2.26437538891355;0.142517128985821]
Robot Arm 1 togoal: 21.4551239119681

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.923492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-1.08029240759648;-8.66414855181461;7.22652367815888]
Robot Arm 1 end: [-8.41200850246739;7.56783610516187;0.110868813294191]
Robot Arm 1 togoal: 18.6254113584105
Mobile Platform 2 Pose: [0.690402918579056;8.06307554312293;2.34834652428657]
Robot Arm 2 end: [7.53829047997186;2.16172435877422;0.142600677425194]
Robot Arm 1 togoal: 21.3868921779474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001212e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-1.08354869162124;-8.66132283672767;7.27259189307195]
Robot Arm 1 end: [-8.44642784001037;7.58552579506062;0.0996742145037037]
Robot Arm 1 togoal: 18.6579889787604
Mobile Platform 2 Pose: [0.706257292060165;8.05225122491677;2.24221640473716]
Robot Arm 2 end: [7.52647542500795;2.05830353930396;0.142686057507779]
Robot Arm 1 togoal: 21.3185312438138

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000995e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-1.0864585174872;-8.65868528296969;7.31374159244133]
Robot Arm 1 end: [-8.49149941679489;7.59921188315172;0.0919276044079975]
Robot Arm 1 togoal: 18.7013791516874
Mobile Platform 2 Pose: [1.03055979486009;7.88720657202289;2.13531276661239]
Robot Arm 2 end: [7.36143925469447;1.95127076549793;0.142773283193615]
Robot Arm 1 togoal: 21.122309409428

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000814e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.190155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-1.08927509079456;-8.65612926866032;7.35208786141183]
Robot Arm 1 end: [-8.53901178099116;7.61259452699243;0.089418883528858]
Robot Arm 1 togoal: 18.7468420594326
Mobile Platform 2 Pose: [1.04844976904611;7.87537795505391;2.02674993586899]
Robot Arm 2 end: [7.34963471547769;1.82539253063628;0.146974234084035]
Robot Arm 1 togoal: 21.0413326590674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000622e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997109e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-1.09197831899831;-8.65366776467353;7.38995894634834]
Robot Arm 1 end: [-8.58685990342046;7.62760310581444;0.0916267298293622]
Robot Arm 1 togoal: 18.7920923805307
Mobile Platform 2 Pose: [1.06613895242752;7.86366629685359;1.91576344408317]
Robot Arm 2 end: [7.36475290231833;1.72110341494379;0.123102641643306]
Robot Arm 1 togoal: 20.9969765834722

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000438e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997160e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-1.09459841861306;-8.65127948928967;7.42950361266346]
Robot Arm 1 end: [-8.63338707163728;7.64577155554993;0.0976463626864112]
Robot Arm 1 togoal: 18.8353773762824
Mobile Platform 2 Pose: [1.08385980106892;7.85200448664174;1.81105833511772]
Robot Arm 2 end: [7.36884416172321;1.61678352648683;0.111544045329668]
Robot Arm 1 togoal: 20.9430889102739

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000274e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997186e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-1.09718324608158;-8.64892715705731;7.47255705772109]
Robot Arm 1 end: [-8.67783020438766;7.66840347500015;0.106415054282648]
Robot Arm 1 togoal: 18.8758770410272
Mobile Platform 2 Pose: [1.10165731018847;7.84035541816568;1.70980691487359]
Robot Arm 2 end: [7.36692728847955;1.51252033264432;0.106017499527144]
Robot Arm 1 togoal: 20.884215650331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997070e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-1.09931861691221;-8.64680192362683;7.52063951827718]
Robot Arm 1 end: [-8.7255779076652;7.69513349922525;0.117473814724191]
Robot Arm 1 togoal: 18.9190264896265
Mobile Platform 2 Pose: [1.11968672200008;7.82863351292901;1.61056659218233]
Robot Arm 2 end: [7.35601990024783;1.40465548824617;0.108940901735051]
Robot Arm 1 togoal: 20.8156552738648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996902e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-1.10172337921911;-8.64452161782643;7.57649918605858]
Robot Arm 1 end: [-8.76820730464724;7.72913382011039;0.129331908217596]
Robot Arm 1 togoal: 18.9562437923462
Mobile Platform 2 Pose: [1.13808152194052;7.81675805435773;1.51097092255259]
Robot Arm 2 end: [7.33746162915976;1.29138287186978;0.119323231730378]
Robot Arm 1 togoal: 20.7375890755036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996268e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983217e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-1.10449351424119;-8.64201425117045;7.64087627367984]
Robot Arm 1 end: [-8.80586310746162;7.77081697658148;0.141269546088634]
Robot Arm 1 togoal: 18.9877170901068
Mobile Platform 2 Pose: [1.15254812682447;7.80686346685587;1.40881301505937]
Robot Arm 2 end: [7.30347993934986;1.23478473333447;0.125786871950108]
Robot Arm 1 togoal: 20.6779453933057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.494638e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-1.10770404076313;-8.63922210764702;7.71407438521288]
Robot Arm 1 end: [-8.83883207424647;7.8204179723007;0.152792259103172]
Robot Arm 1 togoal: 19.0137887902274
Mobile Platform 2 Pose: [1.16738460488147;7.7967215206215;1.30405227261036]
Robot Arm 2 end: [7.29321500884538;1.16843755587087;0.117832532273071]
Robot Arm 1 togoal: 20.6339080574677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-1.11137781093113;-8.6361179870328;7.7960893578363]
Robot Arm 1 end: [-8.86815586424934;7.8788819790782;0.163254197080852]
Robot Arm 1 togoal: 19.0354910901093
Mobile Platform 2 Pose: [1.18237903852585;7.78648071694119;1.20147037836746]
Robot Arm 2 end: [7.22876281024167;1.04401871087505;0.158789265757612]
Robot Arm 1 togoal: 20.5099057857955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-1.11551613842784;-8.63269782559781;7.88680747075273]
Robot Arm 1 end: [-8.89411644798155;7.94619033903083;0.172437404404196]
Robot Arm 1 togoal: 19.0531989846224
Mobile Platform 2 Pose: [1.19812734568526;7.77573207885816;1.07930969095722]
Robot Arm 2 end: [7.2173509583791;0.924496222587517;0.158772560694871]
Robot Arm 1 togoal: 20.4361800573816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-1.12010643027622;-8.62896782849111;7.98597066751895]
Robot Arm 1 end: [-8.91694966850504;8.02223244189344;0.180174856313605]
Robot Arm 1 togoal: 19.067258883767
Mobile Platform 2 Pose: [1.21384200042559;7.76500652976992;0.956337895792794]
Robot Arm 2 end: [7.20597577325266;0.804153280855108;0.158758287521872]
Robot Arm 1 togoal: 20.362490832636

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997355e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-1.12513533331603;-8.62493719561652;8.09324035769104]
Robot Arm 1 end: [-8.93653573777695;8.10650544032355;0.186407232328447]
Robot Arm 1 togoal: 19.0777054503719
Mobile Platform 2 Pose: [1.22952188573937;7.7543048314675;0.832544176241477]
Robot Arm 2 end: [7.19463789897855;0.682978765736397;0.158746450217281]
Robot Arm 1 togoal: 20.2888477376134

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-1.13013952042691;-8.62083875198991;8.20822741221748]
Robot Arm 1 end: [-8.95151515127619;8.1930088991213;0.190234486855815]
Robot Arm 1 togoal: 19.0839220344949
Mobile Platform 2 Pose: [1.24516585656466;7.74362776491197;0.707917590631721]
Robot Arm 2 end: [7.18333799744647;0.560961428530661;0.158737051144502]
Robot Arm 1 togoal: 20.2152608534054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997270e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-1.13591153977911;-8.61623479686309;8.3316480628742]
Robot Arm 1 end: [-8.96205856342305;8.29140389059831;0.190659376364982]
Robot Arm 1 togoal: 19.0853025074962
Mobile Platform 2 Pose: [1.26077273924758;7.73297613058;0.582447077667122]
Robot Arm 2 end: [7.17207674929345;0.438089896144297;0.158730090719719]
Robot Arm 1 togoal: 20.1417407358288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997923e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.035716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-1.14243917404141;-8.61114944109308;8.46315504900067]
Robot Arm 1 end: [-8.96959000992052;8.4025483100701;0.188444639545249]
Robot Arm 1 togoal: 19.0833147234649
Mobile Platform 2 Pose: [1.27640023298701;7.72232129787525;0.456121459671674]
Robot Arm 2 end: [7.16402678376632;0.260487351679014;0.168410136031598]
Robot Arm 1 togoal: 20.0427272462587

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000259e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-1.14546286190267;-8.60773816572052;8.60241284912212]
Robot Arm 1 end: [-8.9729471077669;8.55022062009078;0.183886252342995]
Robot Arm 1 togoal: 19.0751790392306
Mobile Platform 2 Pose: [1.29161225644638;7.71195178707787;0.325349419951952]
Robot Arm 2 end: [7.16860079611884;0.0824922828580487;0.174971016314984]
Robot Arm 1 togoal: 19.9556833497454

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-1.15463926385019;-8.60148231010836;8.73758651800761]
Robot Arm 1 end: [-8.96241067839656;8.68244999362726;0.181684082799544]
Robot Arm 1 togoal: 19.055254495323
Mobile Platform 2 Pose: [1.30615125149178;7.70204539593749;0.195223668384338]
Robot Arm 2 end: [7.18041882906415;-0.113494954781155;0.187532477707707]
Robot Arm 1 togoal: 19.8667538562345

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000341e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-1.16378942848769;-8.59524450147321;8.87253807409813]
Robot Arm 1 end: [-8.9553733567034;8.81635021319233;0.181075363371445]
Robot Arm 1 togoal: 19.0395011473955
Mobile Platform 2 Pose: [1.31978689218242;7.69274260698889;0.0640551601147882]
Robot Arm 2 end: [7.23647211103663;-0.208466135373179;0.111769457937705]
Robot Arm 1 togoal: 19.8735008737102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998424e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-1.17299354801037;-8.58896989367762;9.00781567304454]
Robot Arm 1 end: [-8.94920355946758;8.95105479527444;0.180867625053388]
Robot Arm 1 togoal: 19.0254712520619
Mobile Platform 2 Pose: [1.33860113775643;7.68097548504815;-0.0484393508368412]
Robot Arm 2 end: [7.19498855362843;-0.351991037056756;0.139278253182923]
Robot Arm 1 togoal: 19.7651426327784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998733e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-1.18227336401667;-8.58264361848579;9.1435469744986]
Robot Arm 1 end: [-8.94321516082251;9.08632605287726;0.180763988497414]
Robot Arm 1 togoal: 19.0125335838597
Mobile Platform 2 Pose: [1.35844170377938;7.66860156111979;-0.178838309057576]
Robot Arm 2 end: [7.15311062916492;-0.49948562032073;0.182682510524833]
Robot Arm 1 togoal: 19.6540090423684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-1.19163475365192;-8.57626165693033;9.27975387068382]
Robot Arm 1 end: [-8.93723118212289;9.22209290348239;0.180689839142005]
Robot Arm 1 togoal: 19.0005340878429
Mobile Platform 2 Pose: [1.37692551628222;7.65691770947447;-0.316235219196345]
Robot Arm 2 end: [7.14698806949369;-0.630652914824314;0.174934262321019]
Robot Arm 1 togoal: 19.5861113452365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004229e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-1.20107927896303;-8.56982294054495;9.41643185195074]
Robot Arm 1 end: [-8.93120641304739;9.35832745662288;0.180626610193162]
Robot Arm 1 togoal: 18.9894423491169
Mobile Platform 2 Pose: [1.39157289016593;7.64717249075698;-0.453036601164048]
Robot Arm 2 end: [7.14034857311927;-0.75101268207882;0.165249724478873]
Robot Arm 1 togoal: 19.5236619112469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004740e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-1.21060731789214;-8.56332721034776;9.55356970892327]
Robot Arm 1 end: [-8.92512943777442;9.49501268168674;0.180569792734666]
Robot Arm 1 togoal: 18.9792594578588
Mobile Platform 2 Pose: [1.40397398987703;7.6385572633588;-0.589102978224364]
Robot Arm 2 end: [7.13392309358476;-0.863004517623026;0.15477466851087]
Robot Arm 1 togoal: 19.4659337992878

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005154e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-1.22021891197142;-8.55677443727227;9.69115432291363]
Robot Arm 1 end: [-8.91899742777845;9.63213400874113;0.180518355677101]
Robot Arm 1 togoal: 18.969994203968
Mobile Platform 2 Pose: [1.41426687347384;7.63098727933711;-0.723882430206867]
Robot Arm 2 end: [7.1288768224353;-0.96227030357734;0.142860097816276]
Robot Arm 1 togoal: 19.4159277103555

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987883e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005441e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-1.22988271349479;-8.55018030742496;9.82917179565776]
Robot Arm 1 end: [-8.91092979148508;9.76393960937226;0.179047793477663]
Robot Arm 1 togoal: 18.9599532122601
Mobile Platform 2 Pose: [1.42267213913435;7.62433034469951;-0.856139127556697]
Robot Arm 2 end: [7.12683515077485;-1.04578527992;0.130440365650027]
Robot Arm 1 togoal: 19.3762767226265

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005593e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-1.23521877209552;-8.54554999478267;9.97464397788519]
Robot Arm 1 end: [-8.89483995876593;9.89898404869433;0.17303163848564]
Robot Arm 1 togoal: 18.9431306808943
Mobile Platform 2 Pose: [1.43000233744959;7.61815739853494;-0.984071207048519]
Robot Arm 2 end: [7.1296885700157;-1.11609180426802;0.119604402343511]
Robot Arm 1 togoal: 19.3471952736579

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005545e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-1.23902954818397;-8.54154167665221;10.1257269023005]
Robot Arm 1 end: [-8.86692912088991;10.0360882162363;0.162523461701008]
Robot Arm 1 togoal: 18.9158075512822
Mobile Platform 2 Pose: [1.43636012145182;7.61240713404396;-1.10598620901684]
Robot Arm 2 end: [7.14009253129977;-1.17083591886008;0.111210020365363]
Robot Arm 1 togoal: 19.3319532934437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005286e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-1.23564423367136;-8.54109220718978;10.2787072279139]
Robot Arm 1 end: [-8.90120026777733;10.2005539887157;0.177417764073293]
Robot Arm 1 togoal: 18.9499553215757
Mobile Platform 2 Pose: [1.44213340671057;7.60688216092124;-1.21989889643165]
Robot Arm 2 end: [7.15970116273786;-1.21255524186753;0.106678954663429]
Robot Arm 1 togoal: 19.3306707951633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004799e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-1.23539997281314;-8.53885248216227;10.4450566393113]
Robot Arm 1 end: [-8.89837048832632;10.3624843439676;0.175915046730653]
Robot Arm 1 togoal: 18.9496452617239
Mobile Platform 2 Pose: [1.44751842226094;7.60149482937689;-1.3243054917955]
Robot Arm 2 end: [7.18888159825188;-1.24422539577198;0.106700931504697]
Robot Arm 1 togoal: 19.3422201769578

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004157e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-1.23680953657393;-8.53570749560948;10.6161901760315]
Robot Arm 1 end: [-8.90034531903733;10.5325402248703;0.176893751676289]
Robot Arm 1 togoal: 18.9555605002
Mobile Platform 2 Pose: [1.45279057064748;7.59612808239397;-1.41844546739589]
Robot Arm 2 end: [7.22584811645889;-1.27075068438744;0.111054162531786]
Robot Arm 1 togoal: 19.3627886475587

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999938e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003417e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-1.2421924449015;-8.53055550751782;10.7898696870109]
Robot Arm 1 end: [-8.78976877878495;10.6707276625787;0.163564068131841]
Robot Arm 1 togoal: 18.8506742231326
Mobile Platform 2 Pose: [1.45799608707187;7.59078570805668;-1.50260360693973]
Robot Arm 2 end: [7.26816575815641;-1.29517539411056;0.118855012038238]
Robot Arm 1 togoal: 19.3889022545819

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002579e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-1.25776071624;-8.51951778543615;10.9725609830158]
Robot Arm 1 end: [-8.69305456801632;10.8337285982082;0.146881372576572]
Robot Arm 1 togoal: 18.7620253293864
Mobile Platform 2 Pose: [1.46285818229837;7.58563161943291;-1.57776484192799]
Robot Arm 2 end: [7.31440620944712;-1.3180268494601;0.129251357198384]
Robot Arm 1 togoal: 19.4191216190943

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000615e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-1.27637968331315;-8.50711695632187;11.1452429807635]
Robot Arm 1 end: [-8.63224130335846;10.9933977414378;0.145458431283536]
Robot Arm 1 togoal: 18.7093391028796
Mobile Platform 2 Pose: [1.46776904034136;7.58048996360546;-1.64518591618159]
Robot Arm 2 end: [7.36085826972064;-1.34309555186557;0.140783929563161]
Robot Arm 1 togoal: 19.4485575528508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991898e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-1.33060808545191;-8.47709760507152;11.3096645629309]
Robot Arm 1 end: [-8.59310724260696;11.1990918771481;0.113781839564844]
Robot Arm 1 togoal: 18.6847992019177
Mobile Platform 2 Pose: [1.47265010399083;7.57541434658779;-1.70641009584399]
Robot Arm 2 end: [7.40600678404589;-1.37050224473347;0.152775704262677]
Robot Arm 1 togoal: 19.4758656135702

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003503e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991102e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-1.38721942756348;-8.44565043931538;11.4987924520625]
Robot Arm 1 end: [-8.5891533486074;11.452721097074;0.0886868956300443]
Robot Arm 1 togoal: 18.7007631296927
Mobile Platform 2 Pose: [1.47702515458206;7.57065067974671;-1.76279877623635]
Robot Arm 2 end: [7.4501976641319;-1.39856841050935;0.16526221413426]
Robot Arm 1 togoal: 19.5021037031902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002976e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990523e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-1.43979183178262;-8.41644860120211;11.7170217056092]
Robot Arm 1 end: [-8.57103874390868;11.7457530664025;0.0974323051482925]
Robot Arm 1 togoal: 18.7071558174522
Mobile Platform 2 Pose: [1.48145815659269;7.5659199560651;-1.81543394587741]
Robot Arm 2 end: [7.49177219590973;-1.42966985383062;0.177270782011403]
Robot Arm 1 togoal: 19.524811435177

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995211e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990204e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-1.46556746126522;-8.40089189753595;11.9528218633642]
Robot Arm 1 end: [-8.50792331064859;12.0373842786909;0.0953571734574976]
Robot Arm 1 togoal: 18.6742233423717
Mobile Platform 2 Pose: [1.48452390844042;7.56193542354892;-1.86512379264433]
Robot Arm 2 end: [7.53183868540577;-1.46091859406209;0.187812813841272]
Robot Arm 1 togoal: 19.5463210610504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995085e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955174e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-1.49155633665196;-8.38518481134294;12.1840997323758]
Robot Arm 1 end: [-8.43948868316013;12.3120884541317;0.100887135796493]
Robot Arm 1 togoal: 18.6380582803895
Mobile Platform 2 Pose: [1.49736990615729;7.55311454618943;-1.91280837745068]
Robot Arm 2 end: [7.59691013690209;-1.5100419532477;0.196640077365624]
Robot Arm 1 togoal: 19.5827618891336

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995931e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-1.52032694443721;-8.36813132002178;12.4125627304514]
Robot Arm 1 end: [-8.45076202477298;12.640805142277;0.140505542001029]
Robot Arm 1 togoal: 18.6898431251535
Mobile Platform 2 Pose: [1.50957668822912;7.54474327497309;-1.95912355551118]
Robot Arm 2 end: [7.63149501619263;-1.56661003971238;0.202792891424629]
Robot Arm 1 togoal: 19.5890110191761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976539e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-1.5431350087154;-8.35465573246854;12.6362611152385]
Robot Arm 1 end: [-8.39889989598819;12.9101455529971;0.154893726419678]
Robot Arm 1 togoal: 18.6776527131473
Mobile Platform 2 Pose: [1.52132465035058;7.53669573451031;-2.00566788810616]
Robot Arm 2 end: [7.65183419640584;-1.64752825892811;0.191238016544678]
Robot Arm 1 togoal: 19.5734335673496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993866e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-1.56715826416483;-8.34073152272923;12.8518861853241]
Robot Arm 1 end: [-8.34023865281523;13.1662637723442;0.171650200100463]
Robot Arm 1 togoal: 18.6603945564065
Mobile Platform 2 Pose: [1.53264026439802;7.52894364657661;-2.05317441852086]
Robot Arm 2 end: [7.65091948621395;-1.68169186793679;0.205664854510782]
Robot Arm 1 togoal: 19.5570783971869

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993911e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-1.58920308918044;-8.32792715113516;13.057174595711]
Robot Arm 1 end: [-8.28490123769514;13.400052224136;0.186741741480315]
Robot Arm 1 togoal: 18.6461313279604
Mobile Platform 2 Pose: [1.53575868638175;7.52528749623921;-2.10248704338689]
Robot Arm 2 end: [7.65602475497174;-1.72801749347834;0.212702397520637]
Robot Arm 1 togoal: 19.5415594913259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960655e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-1.60978259092204;-8.31593789608491;13.25222667028]
Robot Arm 1 end: [-8.23101339607929;13.6064746000496;0.193338013602945]
Robot Arm 1 togoal: 18.6316718208257
Mobile Platform 2 Pose: [1.54054891784981;7.52080427103341;-2.15304047265704]
Robot Arm 2 end: [7.66152424291955;-1.78257154093102;0.212803194040031]
Robot Arm 1 togoal: 19.5234973287645

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952016e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-1.63038243012287;-8.30397232430408;13.4427710476926]
Robot Arm 1 end: [-8.18472690796436;13.8102496533122;0.201704502791987]
Robot Arm 1 togoal: 18.6264015780494
Mobile Platform 2 Pose: [1.54642130755769;7.51579858233105;-2.20347987614276]
Robot Arm 2 end: [7.66599444248303;-1.83711059508837;0.212627083082914]
Robot Arm 1 togoal: 19.5046651539672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994882e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.144438e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-1.65076206939597;-8.29219284323794;13.630301282171]
Robot Arm 1 end: [-8.13922040702208;14.0078964071316;0.209098826174019]
Robot Arm 1 togoal: 18.6229867442017
Mobile Platform 2 Pose: [1.54991834123286;7.5119984017302;-2.25393605477561]
Robot Arm 2 end: [7.66783625256076;-1.88905318202553;0.212841060145589]
Robot Arm 1 togoal: 19.484639487125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.135714e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-1.67096693570601;-8.28056802981516;13.8133879756715]
Robot Arm 1 end: [-8.09773325792257;14.2019470193752;0.216919776213294]
Robot Arm 1 togoal: 18.6248471955779
Mobile Platform 2 Pose: [1.56104092861534;7.50438365071221;-2.30480518166927]
Robot Arm 2 end: [7.66475208830782;-1.94140941907242;0.212960941659972]
Robot Arm 1 togoal: 19.4600954435104

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995361e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-1.69087855363498;-8.26917502892003;13.992251539529]
Robot Arm 1 end: [-8.06084815981957;14.3948850597499;0.226676773194488]
Robot Arm 1 togoal: 18.6328882236737
Mobile Platform 2 Pose: [1.57219655471545;7.49674790248522;-2.35607374563814]
Robot Arm 2 end: [7.65936439830738;-1.99188559666198;0.213821470979123]
Robot Arm 1 togoal: 19.4342934771473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995643e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-1.71032995583463;-8.25812264437565;14.167492792396]
Robot Arm 1 end: [-8.02681140256552;14.5856719501312;0.238005871174153]
Robot Arm 1 togoal: 18.6451170688867
Mobile Platform 2 Pose: [1.58343085850415;7.48905833728899;-2.4070060706554]
Robot Arm 2 end: [7.65392538573729;-2.04203727196853;0.214722697095055]
Robot Arm 1 togoal: 19.4086736460275

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995944e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-1.72925268328358;-8.24745342491461;14.3391512922861]
Robot Arm 1 end: [-7.99471420189711;14.7729033127006;0.250274861218703]
Robot Arm 1 togoal: 18.6602542217055
Mobile Platform 2 Pose: [1.59474437740783;7.48131459638973;-2.45763010125253]
Robot Arm 2 end: [7.64845255612677;-2.09189438102165;0.215638169924074]
Robot Arm 1 togoal: 19.3832397089391

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-1.74829113044133;-8.23684293620682;14.5071594338088]
Robot Arm 1 end: [-8.01374706040006;14.9762627905338;0.252721486131598]
Robot Arm 1 togoal: 18.7314389437988
Mobile Platform 2 Pose: [1.60613688589265;7.47351683366081;-2.50795221567997]
Robot Arm 2 end: [7.64294592537152;-2.1414635426091;0.216567923499954]
Robot Arm 1 togoal: 19.3579874785559

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.902004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-1.75330835472716;-8.23343940856883;14.6919532619971]
Robot Arm 1 end: [-8.00878572987659;15.1640914417702;0.250826669098127]
Robot Arm 1 togoal: 18.7775808924927
Mobile Platform 2 Pose: [1.61761257328006;7.46566299536342;-2.55797878510655]
Robot Arm 2 end: [7.63060040217665;-2.18954646231666;0.211115405913263]
Robot Arm 1 togoal: 19.3276240820068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.696467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-1.75879419461817;-8.22971637516995;14.8742710887399]
Robot Arm 1 end: [-8.00538060370229;15.3474124164239;0.250585195109076]
Robot Arm 1 togoal: 18.8255811068872
Mobile Platform 2 Pose: [1.62938164577744;7.45761950220814;-2.60886097791941]
Robot Arm 2 end: [7.58072310911544;-2.23222283780529;0.188709207146715]
Robot Arm 1 togoal: 19.2664263899048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.430356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-1.76413605882512;-8.22609151032257;15.0578940311545]
Robot Arm 1 end: [-8.00206090211589;15.5320324873547;0.250352419874862]
Robot Arm 1 togoal: 18.8757019852909
Mobile Platform 2 Pose: [1.67983897559395;7.43007283054528;-2.66414392230662]
Robot Arm 2 end: [7.40985234092507;-2.29043490458739;0.128859116416469]
Robot Arm 1 togoal: 19.0913176269466

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.523063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-1.76933335323755;-8.22256521676042;15.2428105791999]
Robot Arm 1 end: [-7.99882692489393;15.717940012974;0.250128306617672]
Robot Arm 1 togoal: 18.9279614202189
Mobile Platform 2 Pose: [1.73175521339596;7.40138544244167;-2.73630166225406]
Robot Arm 2 end: [7.24108430903654;-2.40422054508665;0.0935628044815901]
Robot Arm 1 togoal: 18.8941374671124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880576e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-1.77438553704115;-8.2191378608833;15.429008547342]
Robot Arm 1 end: [-7.9956789409985;15.9051226777195;0.249912814942724]
Robot Arm 1 togoal: 18.9823759581995
Mobile Platform 2 Pose: [1.78302936351565;7.3727117104944;-2.82545263929831]
Robot Arm 2 end: [7.08719323514215;-2.57000375869644;0.0783442361164311]
Robot Arm 1 togoal: 18.6884529176157

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.907359e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-1.779292123731;-8.215809772069;15.6164750908477]
Robot Arm 1 end: [-7.99261718802753;16.093567508516;0.249705900868183]
Robot Arm 1 togoal: 19.0389607631886
Mobile Platform 2 Pose: [1.83714040447779;7.34241064785151;-2.93006854706942]
Robot Arm 2 end: [6.96146787683637;-2.77579387019975;0.0797049380641716]
Robot Arm 1 togoal: 18.4921031307517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-1.7840526820062;-8.21258124206601;15.805196723722]
Robot Arm 1 end: [-7.98964187173382;16.2832608928776;0.249507516863088]
Robot Arm 1 togoal: 19.0977295837981
Mobile Platform 2 Pose: [1.89208182251745;7.31157101644847;-3.04378177985971]
Robot Arm 2 end: [6.87353373401734;-3.00354740834513;0.0902088956353979]
Robot Arm 1 togoal: 18.3224903526956

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949497e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-1.78866683654137;-8.20945252447015;15.9951593382268]
Robot Arm 1 end: [-7.98675316561515;16.4741885985864;0.249317611893239]
Robot Arm 1 togoal: 19.1586947245123
Mobile Platform 2 Pose: [1.94363147020378;7.28237172839827;-3.15845966615731]
Robot Arm 2 end: [6.82363896000268;-3.23242138734255;0.103971251271925]
Robot Arm 1 togoal: 18.1890878993351

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-1.79313426863157;-8.20642383428762;16.1863482259041]
Robot Arm 1 end: [-7.98395121057772;16.6663357948727;0.249136131474907]
Robot Arm 1 togoal: 19.2218670210389
Mobile Platform 2 Pose: [1.98566315158991;7.25792526266431;-3.26760591516346]
Robot Arm 2 end: [6.79980638730083;-3.4484500384281;0.121639232364862]
Robot Arm 1 togoal: 18.0863073098895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980432e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-1.79745471670732;-8.20349534758643;16.3787481000215]
Robot Arm 1 end: [-7.9812361146742;16.8596870750117;0.248963017736249]
Robot Arm 1 togoal: 19.2872558199188
Mobile Platform 2 Pose: [2.02553910661921;7.23457141400888;-3.3691708718617]
Robot Arm 2 end: [6.80339700397752;-3.6557425780759;0.140900541497496]
Robot Arm 1 togoal: 18.014117453383

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-1.8016279767177;-8.20066720123783;16.5723431193487]
Robot Arm 1 end: [-7.97860795291793;17.0542264802457;0.248798209486205]
Robot Arm 1 togoal: 19.3548689624888
Mobile Platform 2 Pose: [2.06382827858407;7.21206173033915;-3.45974451765174]
Robot Arm 2 end: [6.83899953991161;-3.84359752613827;0.157872398606692]
Robot Arm 1 togoal: 17.9808596508341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-1.80565390237997;-8.19793949274875;16.7671169131663]
Robot Arm 1 end: [-7.97606676717387;17.249937524933;0.248641642290652]
Robot Arm 1 togoal: 19.4247127732628
Mobile Platform 2 Pose: [2.08175786605877;7.19984100487615;-3.53613165013304]
Robot Arm 2 end: [6.94261594755114;-3.92065751414534;0.104416601266779]
Robot Arm 1 togoal: 18.0559505075778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-1.80953240529475;-8.19531228018573;16.9630526074007]
Robot Arm 1 end: [-7.97361256612665;17.4468032228156;0.248493248555529]
Robot Arm 1 togoal: 19.496792052772
Mobile Platform 2 Pose: [2.09876217955448;7.18824872751793;-3.59634761910965]
Robot Arm 2 end: [7.03011233172794;-3.97412138215761;0.0739098664553361]
Robot Arm 1 togoal: 18.1226380254875

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-1.81326345492703;-8.19278558219031;17.1601328517756]
Robot Arm 1 end: [-7.97124532532566;17.6448061142971;0.24835295761662]
Robot Arm 1 togoal: 19.5711100748711
Mobile Platform 2 Pose: [2.11523451624836;7.17707450752283;-3.64715161954742]
Robot Arm 2 end: [7.07938904166207;-4.09250133003416;0.114276848146817]
Robot Arm 1 togoal: 18.1268568868973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-1.81684707845362;-8.1903593780855;17.3583398478634]
Robot Arm 1 end: [-7.96896498730666;17.8439282946116;0.248220695835649]
Robot Arm 1 togoal: 19.6476685884908
Mobile Platform 2 Pose: [2.13104317776184;7.16638488715492;-3.69423355320777]
Robot Arm 2 end: [7.14189876356914;-4.19515360669593;0.170379259226679]
Robot Arm 1 togoal: 18.1484189739965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961252e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-1.8202833604785;-8.18803360807204;17.5576553779166]
Robot Arm 1 end: [-7.96677146178881;18.0441514427666;0.248096386702286]
Robot Arm 1 togoal: 19.7264678237909
Mobile Platform 2 Pose: [2.14509122539993;7.1568351345933;-3.73386929390932]
Robot Arm 2 end: [7.22381631698284;-4.28355286939929;0.234126635914661]
Robot Arm 1 togoal: 18.1932311416447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-1.82357244261849;-8.18580817351321;17.7580608343584]
Robot Arm 1 end: [-7.96466462594598;18.2454568511347;0.247979950941664]
Robot Arm 1 togoal: 19.8075065026354
Mobile Platform 2 Pose: [2.15748114381594;7.14840812837415;-3.75997378871118]
Robot Arm 2 end: [7.32526742185046;-4.27874958940277;0.237252858719444]
Robot Arm 1 togoal: 18.2905878588453

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-1.82671452296194;-8.18368293730593;17.9595372498062]
Robot Arm 1 end: [-7.96264432475025;18.447825455571;0.247871306626966]
Robot Arm 1 togoal: 19.8907818532899
Mobile Platform 2 Pose: [2.168228179785;7.14109599254439;-3.7816138919958]
Robot Arm 2 end: [7.4166901346794;-4.23060575618693;0.229427281388802]
Robot Arm 1 togoal: 18.3928019963094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962279e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-1.79188552012621;-8.20056989745263;18.1620653208278]
Robot Arm 1 end: [-7.98012035542716;18.6482527070866;0.245510074538815]
Robot Arm 1 togoal: 19.9925955651394
Mobile Platform 2 Pose: [2.17713090350401;7.13503612757589;-3.8061053642352]
Robot Arm 2 end: [7.48670517608842;-4.16878657176537;0.222076264416172]
Robot Arm 1 togoal: 18.4790451818249

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-1.75215707567929;-8.21995447402996;18.3675110982128]
Robot Arm 1 end: [-8.00027381276596;18.8560171038414;0.247153921147629]
Robot Arm 1 togoal: 20.1012830463801
Mobile Platform 2 Pose: [2.18440090858972;7.13010366764334;-3.84127631923543]
Robot Arm 2 end: [7.52910222146312;-4.10158493674794;0.211593215532353]
Robot Arm 1 togoal: 18.5411777154557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-1.70800098260464;-8.24159836235625;18.5735586042395]
Robot Arm 1 end: [-8.02250928253416;19.0641428310685;0.248243957491716]
Robot Arm 1 togoal: 20.2135952775547
Mobile Platform 2 Pose: [2.19106957514705;7.12575825028452;-3.89585197774128]
Robot Arm 2 end: [7.54747815243071;-4.09460811877032;0.202411112370654]
Robot Arm 1 togoal: 18.5614201890778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-1.65992947492443;-8.26523372964733;18.7800096528543]
Robot Arm 1 end: [-8.04656625760534;19.271684555445;0.24813860925218]
Robot Arm 1 togoal: 20.3288677575972
Mobile Platform 2 Pose: [2.19706526288413;7.12187285115461;-3.95974759281432]
Robot Arm 2 end: [7.56358471662464;-4.10202769051235;0.199951850099786]
Robot Arm 1 togoal: 18.5744650979749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-1.60851042346654;-8.29056716519281;18.9871120013296]
Robot Arm 1 end: [-8.07234197465335;19.47987757048;0.24804236682912]
Robot Arm 1 togoal: 20.4474459122883
Mobile Platform 2 Pose: [2.20243488012278;7.11841889132781;-4.03517406979763]
Robot Arm 2 end: [7.57482130334201;-4.12079487815999;0.19947829610274]
Robot Arm 1 togoal: 18.5791769549392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-1.55447274580857;-8.31723426875374;19.1948405141048]
Robot Arm 1 end: [-8.09946398564214;19.6886962569693;0.247953250386955]
Robot Arm 1 togoal: 20.5689591315182
Mobile Platform 2 Pose: [2.2072433912442;7.11535305288525;-4.1235902848525]
Robot Arm 2 end: [7.57988377101966;-4.15142126879998;0.199574791854208]
Robot Arm 1 togoal: 18.5742927498823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-1.49877846757755;-8.34475390891337;19.4031696849069]
Robot Arm 1 end: [-8.1274426398545;19.8981118651225;0.247870763880757]
Robot Arm 1 togoal: 20.6929372783942
Mobile Platform 2 Pose: [2.21157255214762;7.1126196495266;-4.22608899586535]
Robot Arm 2 end: [7.57810291004734;-4.19538414068168;0.199788861068396]
Robot Arm 1 togoal: 18.5587948440652

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-1.44265503703909;-8.37251220779501;19.6120782420042]
Robot Arm 1 end: [-8.15565350054218;20.1080985149914;0.247794732605908]
Robot Arm 1 togoal: 20.8188006684689
Mobile Platform 2 Pose: [2.2141685133976;7.11082764653923;-4.3435190469628]
Robot Arm 2 end: [7.58489652073437;-4.31857480943958;0.205744717546073]
Robot Arm 1 togoal: 18.5266584763387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-1.38450821529673;-8.40130611976185;19.8215507085438]
Robot Arm 1 end: [-8.18447121587168;20.3192170778218;0.247282713251312]
Robot Arm 1 togoal: 20.9471618656714
Mobile Platform 2 Pose: [2.21663338372909;7.10912529459156;-4.46308866333599]
Robot Arm 2 end: [7.58324104642342;-4.44017205265784;0.205003428539806]
Robot Arm 1 togoal: 18.4882097423404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-1.32922342108017;-8.42866094678506;20.0305480761139]
Robot Arm 1 end: [-8.21230427518519;20.5292329058406;0.24724465769352]
Robot Arm 1 togoal: 21.075465007193
Mobile Platform 2 Pose: [2.21901782404837;7.10747771035457;-4.5837485314236]
Robot Arm 2 end: [7.58163148991439;-4.56285486766506;0.204267758809292]
Robot Arm 1 togoal: 18.4502058750631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-1.36985892473537;-8.40808145153565;20.2401588422649]
Robot Arm 1 end: [-8.19160207101634;20.7388656938558;0.247209215229851]
Robot Arm 1 togoal: 21.1631794696412
Mobile Platform 2 Pose: [2.22131968954344;7.10588635833644;-4.70550131916263]
Robot Arm 2 end: [7.58006947392771;-4.68662552413791;0.203537812908032]
Robot Arm 1 togoal: 18.4126722026784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-1.33631528332131;-8.42463529077085;20.4515373191099]
Robot Arm 1 end: [-8.20815877063929;20.9517849731345;0.247192012280583]
Robot Arm 1 togoal: 21.2861745436835
Mobile Platform 2 Pose: [2.22353682044134;7.10435271330902;-4.82834931963917]
Robot Arm 2 end: [7.57855663928441;-4.81148591368316;0.20281369572878]
Robot Arm 1 togoal: 18.3756346033874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-1.31988064287263;-8.43264085644298;20.6628406052341]
Robot Arm 1 end: [-8.21649903706623;21.1636181252684;0.247415847444149]
Robot Arm 1 togoal: 21.4030190255463
Mobile Platform 2 Pose: [2.22566704339246;7.10287825936425;-4.95229443895325]
Robot Arm 2 end: [7.57709464398323;-4.93743753791162;0.202095512453275]
Robot Arm 1 togoal: 18.3391194950401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-1.32133188955532;-8.43174589902783;20.875426445344]
Robot Arm 1 end: [-8.21528563248429;21.3772841372908;0.2471649077523]
Robot Arm 1 togoal: 21.5142250814637
Mobile Platform 2 Pose: [2.22770817293501;7.10146448891594;-5.07733818419373]
Robot Arm 2 end: [7.57568516221172;-5.06448149663332;0.201383368498974]
Robot Arm 1 togoal: 18.3031538231092

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-1.34280447422224;-8.42084743970982;21.0882502093793]
Robot Arm 1 end: [-8.20431366300078;21.5905380900524;0.247353232651186]
Robot Arm 1 togoal: 21.6185056200897
Mobile Platform 2 Pose: [2.22965801304064;7.10011290164613;-5.20348165156001]
Robot Arm 2 end: [7.5743298832902;-5.1926184762106;0.200677369462754]
Robot Arm 1 togoal: 18.2677650469533

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-1.38807280024649;-8.3980894651552;21.3023644229742]
Robot Arm 1 end: [-8.18087310639984;21.805487653838;0.247162119086628]
Robot Arm 1 togoal: 21.714887717941
Mobile Platform 2 Pose: [2.23155727356448;7.09880354661213;-5.3307255131003]
Robot Arm 2 end: [7.55448768923099;-5.28751985909572;0.199639374301829]
Robot Arm 1 togoal: 18.2239814808105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-1.44971649022422;-8.36715370633332;21.5169656631346]
Robot Arm 1 end: [-8.14929181252815;22.0206018540947;0.247168602926387]
Robot Arm 1 togoal: 21.8062905754082
Mobile Platform 2 Pose: [2.23340368508203;7.09753293765676;-5.47142428804838]
Robot Arm 2 end: [7.5315762987803;-5.39843515437692;0.198537210358343]
Robot Arm 1 togoal: 18.1735927757396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-1.59768254472665;-8.29308048576864;21.732534825791]
Robot Arm 1 end: [-8.07388804373351;22.2359364057699;0.247019465763879]
Robot Arm 1 togoal: 21.8633619360123
Mobile Platform 2 Pose: [2.23530348428901;7.09623381163188;-5.6261951822815]
Robot Arm 2 end: [7.49073485131212;-5.51089820756224;0.197318224417561]
Robot Arm 1 togoal: 18.1060756097168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003304e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-1.72572269307803;-8.22900070250696;21.9497674985602]
Robot Arm 1 end: [-8.00856755499366;22.4535632394191;0.247241034699076]
Robot Arm 1 togoal: 21.9322184158982
Mobile Platform 2 Pose: [2.23749610571278;7.09474597020919;-5.80281130164393]
Robot Arm 2 end: [7.44008938085936;-5.64191897450346;0.196994680634685]
Robot Arm 1 togoal: 18.0250541725362

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002532e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-1.72600288461223;-8.22882571907246;22.168130731053]
Robot Arm 1 end: [-8.00836164186145;22.6726254859083;0.247551764531706]
Robot Arm 1 togoal: 22.0571574809307
Mobile Platform 2 Pose: [2.27610649200025;7.07496294599567;-6.00450539821734]
Robot Arm 2 end: [7.3185341699987;-5.79061686316579;0.206191661402662]
Robot Arm 1 togoal: 17.87118499589

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008266e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-1.72609787255334;-8.22877696203571;22.3873605945505]
Robot Arm 1 end: [-8.0081536192797;22.8939158837294;0.248876826298071]
Robot Arm 1 togoal: 22.1847912084233
Mobile Platform 2 Pose: [2.31587017240187;7.05431088137268;-6.24394155458858]
Robot Arm 2 end: [7.22300426273711;-6.06323497505595;0.21148005050116]
Robot Arm 1 togoal: 17.7156664338099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008386e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-1.72606608814489;-8.22881461013435;22.6064781367949]
Robot Arm 1 end: [-8.0082888760869;23.1155261025194;0.251276722843564]
Robot Arm 1 togoal: 22.3142934100772
Mobile Platform 2 Pose: [2.34377445220054;7.03986265823315;-6.49367010155172]
Robot Arm 2 end: [7.15909051935484;-6.34930887277419;0.221716780108635]
Robot Arm 1 togoal: 17.5911929681676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007524e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-1.72580071267983;-8.22901153003225;22.8256219258769]
Robot Arm 1 end: [-8.00857392421778;23.3386194299951;0.255180753695611]
Robot Arm 1 togoal: 22.4461533775071
Mobile Platform 2 Pose: [2.36571942580027;7.02835097631066;-6.75002139551264]
Robot Arm 2 end: [7.1150045838321;-6.64277510182057;0.228381018774114]
Robot Arm 1 togoal: 17.4890023944242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006192e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-1.72530499959932;-8.22936553672926;23.0439382523712]
Robot Arm 1 end: [-8.00899170663167;23.5622838545125;0.260615381910714]
Robot Arm 1 togoal: 22.5798116693965
Mobile Platform 2 Pose: [2.38651032764555;7.01727845413004;-7.01351636437078]
Robot Arm 2 end: [7.08018762903316;-6.94708481533155;0.232952688203881]
Robot Arm 1 togoal: 17.3986254182674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003551e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-1.72458343839987;-8.22987360056252;23.2605683928522]
Robot Arm 1 end: [-8.00952055572838;23.7855791300677;0.2676125473916]
Robot Arm 1 togoal: 22.7146649260982
Mobile Platform 2 Pose: [2.40285022257437;7.00833973023724;-7.2838459680987]
Robot Arm 2 end: [7.05676104605733;-7.26443234127959;0.234476851725837]
Robot Arm 1 togoal: 17.3225739891271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000915e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-1.72364232616193;-8.23053145680316;23.4746485812497]
Robot Arm 1 end: [-8.01013426224896;24.0075336739159;0.276203292968817]
Robot Arm 1 togoal: 22.8500646522489
Mobile Platform 2 Pose: [2.41431166279227;7.00171267729584;-7.5585937155598]
Robot Arm 2 end: [7.04122210784423;-7.58706697365852;0.232684836354758]
Robot Arm 1 togoal: 17.2593526147236

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956286e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-1.72249030671495;-8.23133323727973;23.6853129250654]
Robot Arm 1 end: [-8.01080244224937;24.2271453900506;0.286410513648518]
Robot Arm 1 togoal: 22.98531883101
Mobile Platform 2 Pose: [2.42349013546563;6.99615377963757;-7.83129093573922]
Robot Arm 2 end: [7.02029061941886;-7.91187335660735;0.230522009835109]
Robot Arm 1 togoal: 17.1963920370357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976447e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-1.7211388339634;-8.23227115371154;23.8916995929202]
Robot Arm 1 end: [-8.01149127971143;24.4433873560135;0.298241085463833]
Robot Arm 1 togoal: 23.1196971389776
Mobile Platform 2 Pose: [2.42556908501094;6.99454714708187;-8.10847719934691]
Robot Arm 2 end: [7.00747977371541;-8.25039947390505;0.222951144627965]
Robot Arm 1 togoal: 17.1464398953597

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-1.71960250764267;-8.23333526807836;24.0929604446369]
Robot Arm 1 end: [-8.01216468604831;24.6552191215681;0.311677879008727]
Robot Arm 1 togoal: 23.252440391727
Mobile Platform 2 Pose: [2.42194071517533;6.99603443243319;-8.37708853460502]
Robot Arm 2 end: [6.9980136591829;-8.58792586632699;0.215215107199668]
Robot Arm 1 togoal: 17.1064752623976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992627e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-1.71789922742504;-8.2345133869662;24.2882740118802]
Robot Arm 1 end: [-8.01278586064557;24.8616039509106;0.326672411190758]
Robot Arm 1 togoal: 23.3827745326466
Mobile Platform 2 Pose: [2.4113307301293;7.00052589221205;-8.63108384475188]
Robot Arm 2 end: [6.99282374460705;-8.94676827856352;0.219581178012911]
Robot Arm 1 togoal: 17.0751001995462

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-1.7160501179722;-8.23579111227982;24.4768613725175]
Robot Arm 1 end: [-8.01331916368954;25.0615316828124;0.343139071374147]
Robot Arm 1 togoal: 23.5099289725105
Mobile Platform 2 Pose: [2.36201663092988;7.02522239036024;-8.87031416119735]
Robot Arm 2 end: [6.9680994682152;-9.18536672719619;0.234906451259956]
Robot Arm 1 togoal: 17.0362573222089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001781e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-1.71407919520273;-8.23715206865672;24.6580040324497]
Robot Arm 1 end: [-8.01373213663662;25.2540460412405;0.360951897796448]
Robot Arm 1 togoal: 23.6331584372086
Mobile Platform 2 Pose: [2.33924398582268;7.03648254438024;-9.11927745406637]
Robot Arm 2 end: [7.11438823157884;-9.53624276327027;0.264444386710695]
Robot Arm 1 togoal: 17.1667203427187

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985483e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-1.67112168581646;-8.2590238517907;24.8310624111294]
Robot Arm 1 end: [-8.03487640402718;25.4384343712638;0.379944713024454]
Robot Arm 1 togoal: 23.7677098163094
Mobile Platform 2 Pose: [2.28455982792384;7.06414737129226;-9.30611828376242]
Robot Arm 2 end: [7.09989313825466;-9.7319365963879;0.279239007058639]
Robot Arm 1 togoal: 17.1470164661866

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-1.6121338302554;-8.28892336267784;24.9953911411162]
Robot Arm 1 end: [-8.06402198091985;25.6137400530389;0.399917483758732]
Robot Arm 1 togoal: 23.9030247997873
Mobile Platform 2 Pose: [2.20050818524479;7.10634978997113;-9.50551629665213]
Robot Arm 2 end: [7.19534204586881;-9.93127334936985;0.271202790520736]
Robot Arm 1 togoal: 17.2408382769444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985307e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-1.55098046514726;-8.31991307336934;25.1505301840368]
Robot Arm 1 end: [-8.09409351840147;25.7792598000744;0.42063942884688]
Robot Arm 1 togoal: 24.0331448936142
Mobile Platform 2 Pose: [2.16928978233186;7.12234074157802;-9.68798301047737]
Robot Arm 2 end: [7.29378293513357;-10.0997841193231;0.263263241143296]
Robot Arm 1 togoal: 17.3397458698957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961997e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-1.47883007789633;-8.35640357547775;25.2961794915348]
Robot Arm 1 end: [-8.12961331949371;25.9345746664918;0.441860823723675]
Robot Arm 1 togoal: 24.1610862810045
Mobile Platform 2 Pose: [2.168227630617;7.12335066768915;-9.85041966110359]
Robot Arm 2 end: [7.39807240857392;-10.2300898805703;0.262413475539515]
Robot Arm 1 togoal: 17.4450538814495

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.934561e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-1.3970428051363;-8.39771014711351;25.4321482943708]
Robot Arm 1 end: [-8.16990489206556;26.0793768721604;0.463327041513025]
Robot Arm 1 togoal: 24.2860140095239
Mobile Platform 2 Pose: [2.16703986235652;7.12446871509815;-9.98849586045362]
Robot Arm 2 end: [7.51565617884746;-10.3077621784616;0.273665930854454]
Robot Arm 1 togoal: 17.5627096774427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.933603e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-1.2864086451295;-8.45343336553092;25.5583785760825]
Robot Arm 1 end: [-8.22481538590649;26.2136163950203;0.484790049603175]
Robot Arm 1 togoal: 24.4151384087117
Mobile Platform 2 Pose: [2.16609804919888;7.12542038777616;-10.100884683486]
Robot Arm 2 end: [7.59709344837829;-10.3725606891091;0.294735680094133]
Robot Arm 1 togoal: 17.6437020911231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-1.1785323887067;-8.50776720886396;25.6749001661519]
Robot Arm 1 end: [-8.2782097459745;26.3371705760907;0.506021885585209]
Robot Arm 1 togoal: 24.5362237217315
Mobile Platform 2 Pose: [2.16635290576516;7.12570428264692;-10.198707599341]
Robot Arm 2 end: [7.67055004632886;-10.4336493278843;0.326553521864422]
Robot Arm 1 togoal: 17.7161816729373

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000355e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-1.0672998184842;-8.56376566158328;25.7819120616224]
Robot Arm 1 end: [-8.3332380633166;26.4502705298208;0.526817098390465]
Robot Arm 1 togoal: 24.6517224151251
Mobile Platform 2 Pose: [2.16700410737923;7.12573268531253;-10.2878537192646]
Robot Arm 2 end: [7.73206203436066;-10.4825437859647;0.3642047596855]
Robot Arm 1 togoal: 17.7763057951815

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000598e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-0.952126920249512;-8.62171828935903;25.879695267994]
Robot Arm 1 end: [-8.39022353240486;26.5532375737501;0.547002817577888]
Robot Arm 1 togoal: 24.7620408338286
Mobile Platform 2 Pose: [2.1682454045674;7.12541918726902;-10.3717838869464]
Robot Arm 2 end: [7.78236619329674;-10.5233519451464;0.404898174609809]
Robot Arm 1 togoal: 17.8250421591544

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000632e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-0.836280643517694;-8.67998966598454;25.9686047420102]
Robot Arm 1 end: [-8.4475174432909;26.6464737600732;0.566440618970288]
Robot Arm 1 togoal: 24.8661853141418
Mobile Platform 2 Pose: [2.16958766611016;7.12502868248365;-10.4540990297573]
Robot Arm 2 end: [7.81696991257594;-10.5664932716617;0.439011294370278]
Robot Arm 1 togoal: 17.8587802229487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-0.722810043943553;-8.73705407709236;26.0490572644216]
Robot Arm 1 end: [-8.50358014650694;26.7304584697157;0.585025664051068]
Robot Arm 1 togoal: 24.9633084105982
Mobile Platform 2 Pose: [2.05431780792406;7.18295020418742;-10.5382535714423]
Robot Arm 2 end: [7.91380519510507;-10.6195473322695;0.527321502675846]
Robot Arm 1 togoal: 17.9520375562536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-0.614500584190602;-8.79151813500526;26.1215145953201]
Robot Arm 1 end: [-8.55700415837895;26.8057252283451;0.602684436200679]
Robot Arm 1 togoal: 25.0527065855058
Mobile Platform 2 Pose: [1.93934039917475;7.24073539146591;-10.6192403682854]
Robot Arm 2 end: [7.9956361111074;-10.6758224735197;0.62541314694889]
Robot Arm 1 togoal: 18.0305776632493

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-0.512591454136065;-8.84276175715336;26.1864651730375]
Robot Arm 1 end: [-8.60717063041358;26.8728432961975;0.619371190361097]
Robot Arm 1 togoal: 25.1342908051477
Mobile Platform 2 Pose: [1.93772988987423;7.24184853512497;-10.6969342256947]
Robot Arm 2 end: [8.00265613575523;-10.7283583549236;0.674403942377492]
Robot Arm 1 togoal: 18.0372630606812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-0.418662880132737;-8.88999496901756;26.2444082469313]
Robot Arm 1 end: [-8.65328387670801;26.9323900335467;0.635064090014614]
Robot Arm 1 togoal: 25.2078502468935
Mobile Platform 2 Pose: [1.93571401193594;7.24324457771593;-10.781337811647]
Robot Arm 2 end: [8.0031294142058;-10.7819403713771;0.703242285091274]
Robot Arm 1 togoal: 18.0386481513811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-0.33472446105479;-8.93221328530984;26.2958473951315]
Robot Arm 1 end: [-8.69432583189387;26.9849410342641;0.64976072197101]
Robot Arm 1 togoal: 25.2730112595635
Mobile Platform 2 Pose: [1.93288379131178;7.24517027029431;-10.8796048844142]
Robot Arm 2 end: [8.00027237462004;-10.8498816762508;0.724137679317065]
Robot Arm 1 togoal: 18.0379348591312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-0.284398155106507;-8.95760615528241;26.3412775647934]
Robot Arm 1 end: [-8.7182097626031;27.0309667450324;0.663473776484998]
Robot Arm 1 togoal: 25.3211483983493
Mobile Platform 2 Pose: [1.92956462154707;7.24743208988155;-10.9919084122514]
Robot Arm 2 end: [7.9930721821367;-10.9260452225153;0.737596262043164]
Robot Arm 1 togoal: 18.0339104944649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-0.274816181275176;-8.96260813703142;26.3812013948474]
Robot Arm 1 end: [-8.72129098238658;27.0710047977608;0.676220215573615]
Robot Arm 1 togoal: 25.3499426771845
Mobile Platform 2 Pose: [1.92582038165196;7.24998702600676;-11.1228225056802]
Robot Arm 2 end: [7.98033509464348;-11.0149518421863;0.74357368611126]
Robot Arm 1 togoal: 18.0257307488809

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-0.273849637493157;-8.96328428269971;26.416107813661]
Robot Arm 1 end: [-8.72002729463642;27.105737440048;0.688023494028552]
Robot Arm 1 togoal: 25.3720224881521
Mobile Platform 2 Pose: [1.9218253588908;7.25271626494818;-11.2765779153394]
Robot Arm 2 end: [7.96089430155993;-11.123040614628;0.743154366878136]
Robot Arm 1 togoal: 18.0127729170963

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-0.272977615491978;-8.96389589523679;26.4464388172406]
Robot Arm 1 end: [-8.71878565721321;27.1356959189766;0.69892494733388]
Robot Arm 1 togoal: 25.3909593851933
Mobile Platform 2 Pose: [1.91780711802113;7.25545853265115;-11.4557461855005]
Robot Arm 2 end: [7.94243478618807;-11.2684013905729;0.748082578975849]
Robot Arm 1 togoal: 18.0038109711233

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-0.272194548190786;-8.96444676715651;26.4726062190737]
Robot Arm 1 end: [-8.71757081760386;27.1613380466451;0.708972972736263]
Robot Arm 1 togoal: 25.4070535237019
Mobile Platform 2 Pose: [1.89628493697926;7.26693164251772;-11.6524531305785]
Robot Arm 2 end: [7.89725922301084;-11.3923309117228;0.709374976980174]
Robot Arm 1 togoal: 17.9696746837937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-0.321364173752634;-8.9400060503681;26.4949969695115]
Robot Arm 1 end: [-8.69085259569214;27.1828730042259;0.718217976222692]
Robot Arm 1 togoal: 25.401644718619
Mobile Platform 2 Pose: [1.88636836651579;7.27238745410953;-11.8938122928525]
Robot Arm 2 end: [7.84557499799658;-11.5918280905225;0.677143668680289]
Robot Arm 1 togoal: 17.936291623042

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-0.426978826910877;-8.88732820241454;26.5140057587575]
Robot Arm 1 end: [-8.63531076102384;27.2006950048425;0.726692042228806]
Robot Arm 1 togoal: 25.372615965669
Mobile Platform 2 Pose: [1.88498739733491;7.27321223967313;-12.1438147959974]
Robot Arm 2 end: [7.77803392740235;-11.8133890841209;0.638521185445941]
Robot Arm 1 togoal: 17.8920551964389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959731e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-0.559344095863388;-8.82126101891238;26.5300123943634]
Robot Arm 1 end: [-8.5661520814416;27.215337312365;0.734426305848986]
Robot Arm 1 togoal: 25.3315720014778
Mobile Platform 2 Pose: [1.8887186658574;7.27179622865017;-12.4102037044332]
Robot Arm 2 end: [7.70164088032099;-12.0608050896213;0.588284318746442]
Robot Arm 1 togoal: 17.8455867377712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-0.683842707636901;-8.75911393840865;26.5433584453755]
Robot Arm 1 end: [-8.50109891020344;27.2273172825828;0.741464882619754]
Robot Arm 1 togoal: 25.2918721457929
Mobile Platform 2 Pose: [1.89938620956618;7.26722983058016;-12.6806119683439]
Robot Arm 2 end: [7.61711987214886;-12.3315548027785;0.534666538716509]
Robot Arm 1 togoal: 17.7980873715371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-0.794764736557975;-8.70374314124565;26.5543410457208]
Robot Arm 1 end: [-8.44307012022037;27.2369773121632;0.747864849510386]
Robot Arm 1 togoal: 25.255850020942
Mobile Platform 2 Pose: [1.91717429944228;7.26017687726533;-12.9431231163669]
Robot Arm 2 end: [7.53678295361339;-12.6060713181063;0.478511438940533]
Robot Arm 1 togoal: 17.7599872318969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975304e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-0.899899942214763;-8.65125475882381;26.5632292942062]
Robot Arm 1 end: [-8.38807280128117;27.2445661572999;0.753682678803287]
Robot Arm 1 togoal: 25.2207267771989
Mobile Platform 2 Pose: [1.95101498246901;7.24599741333816;-13.196214291902]
Robot Arm 2 end: [7.4795575436549;-12.863599551709;0.416771054914001]
Robot Arm 1 togoal: 17.7469562426253

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-0.992799020353024;-8.60487441651593;26.5702716446595]
Robot Arm 1 end: [-8.33940352378499;27.2503733135674;0.758968433340485]
Robot Arm 1 togoal: 25.1890808322623
Mobile Platform 2 Pose: [1.99654789821926;7.22434114648483;-13.4478682801631]
Robot Arm 2 end: [7.39105646128666;-13.1432843841276;0.333354321788054]
Robot Arm 1 togoal: 17.7126955547747

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-1.08251872724465;-8.56007472171687;26.5756915829106]
Robot Arm 1 end: [-8.29242117473077;27.2545885969206;0.76377171672916]
Robot Arm 1 togoal: 25.1576408821596
Mobile Platform 2 Pose: [2.03208801661025;7.20617759054006;-13.6979770005245]
Robot Arm 2 end: [7.28872676710561;-13.4240489905826;0.27556886189358]
Robot Arm 1 togoal: 17.6684827712316

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000285e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-1.14586558582183;-8.52845322310566;26.5796955009213]
Robot Arm 1 end: [-8.25899244398893;27.2575332536979;0.768135161600817]
Robot Arm 1 togoal: 25.1352355178349
Mobile Platform 2 Pose: [2.09248710700352;7.17503080020787;-13.9437718436615]
Robot Arm 2 end: [7.19208781302551;-13.7372197505658;0.225677218244477]
Robot Arm 1 togoal: 17.6412175300837

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000975e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [-1.17538129100804;-8.51374002642604;26.5824585192282]
Robot Arm 1 end: [-8.24292794956176;27.2594221880171;0.772107759617724]
Robot Arm 1 togoal: 25.1247470439968
Mobile Platform 2 Pose: [2.14107377609577;7.15030384608951;-14.1600874966312]
Robot Arm 2 end: [7.13125236109359;-14.0363745588316;0.194751644534889]
Robot Arm 1 togoal: 17.6502425405157

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [-1.17525873610491;-8.51383962054971;26.5841306430224]
Robot Arm 1 end: [-8.24207498303341;27.2603879184621;0.775737325366197]
Robot Arm 1 togoal: 25.1246832265923
Mobile Platform 2 Pose: [2.18128998086927;7.1303611413055;-14.3471347095203]
Robot Arm 2 end: [7.0827248087004;-14.3152925942453;0.18317792487116]
Robot Arm 1 togoal: 17.6700595632397

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960702e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [-1.17516601959067;-8.51391877415702;26.5848432031332]
Robot Arm 1 end: [-8.24124390672368;27.2604212966034;0.77906566900763]
Robot Arm 1 togoal: 25.1240042420059
Mobile Platform 2 Pose: [2.17936525108305;7.13201652203351;-14.5072565587376]
Robot Arm 2 end: [7.18929942761786;-14.4319208444315;0.155068773080277]
Robot Arm 1 togoal: 17.8039238736938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [-1.17510039277476;-8.51397936211202;26.5847248732623]
Robot Arm 1 end: [-8.24043324677726;27.2596514479849;0.782118797689474]
Robot Arm 1 togoal: 25.1227967903096
Mobile Platform 2 Pose: [2.17728103680288;7.13380006519579;-14.6814172697562]
Robot Arm 2 end: [7.32145528774099;-14.5672633827575;0.152161069422462]
Robot Arm 1 togoal: 17.9657014366418

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958543e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [-1.17505930074264;-8.51402312715661;26.5838926655691]
Robot Arm 1 end: [-8.23964157380895;27.2581952464675;0.784920875375518]
Robot Arm 1 togoal: 25.1211393130153
Mobile Platform 2 Pose: [2.17470947096137;7.13593727335814;-14.8683942384529]
Robot Arm 2 end: [7.45377472867878;-14.719883150536;0.184679421628337]
Robot Arm 1 togoal: 18.1300110844522

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962016e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [-1.14699790368317;-8.52807292061516;26.582452794854]
Robot Arm 1 end: [-8.25321851871238;27.2562929195094;0.787494296796291]
Robot Arm 1 togoal: 25.1296172321109
Mobile Platform 2 Pose: [2.17203560223282;7.13824510000859;-15.0629625021344]
Robot Arm 2 end: [7.58268021428797;-14.881012389074;0.25381792989075]
Robot Arm 1 togoal: 18.2915465682652

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996101e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [-1.09521395793187;-8.55398049858821;26.5804874523553]
Robot Arm 1 end: [-8.27896022672605;27.2540036187306;0.789871381996178]
Robot Arm 1 togoal: 25.1466812303625
Mobile Platform 2 Pose: [2.1692950527899;7.14069439401857;-15.2622409609743]
Robot Arm 2 end: [7.67976085243744;-15.0540511242291;0.339175700688757]
Robot Arm 1 togoal: 18.4259079895712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [-1.02007537558161;-8.59156243894132;26.5780667561563]
Robot Arm 1 end: [-8.31667483489238;27.2513930153174;0.792079362150912]
Robot Arm 1 togoal: 25.1722558240696
Mobile Platform 2 Pose: [2.16650191473977;7.14325150918095;-15.4636386963806]
Robot Arm 2 end: [7.73691697332799;-15.2382928474176;0.409491044492873]
Robot Arm 1 togoal: 18.5276384850366

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [-0.942540590314283;-8.63034000245502;26.5752489742457]
Robot Arm 1 end: [-8.35563379238295;27.2484158783083;0.794142585047253]
Robot Arm 1 togoal: 25.1985210663805
Mobile Platform 2 Pose: [2.16365074798331;7.1458928542064;-15.665847207194]
Robot Arm 2 end: [7.77758759835735;-15.4285332505681;0.477014129878568]
Robot Arm 1 togoal: 18.6172366522748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999324e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [-0.866855796748697;-8.66819033985339;26.5720920984222]
Robot Arm 1 end: [-8.39366261423929;27.2451087163932;0.796073937095347]
Robot Arm 1 togoal: 25.2239195368873
Mobile Platform 2 Pose: [2.16072616886736;7.14861972437811;-15.8682409326517]
Robot Arm 2 end: [7.80670914931134;-15.6224180916403;0.544346531549124]
Robot Arm 1 togoal: 18.6987787815892

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999107e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [-0.785335685151403;-8.70895629290173;26.5686497052186]
Robot Arm 1 end: [-8.43469306465702;27.241561122905;0.797883430789545]
Robot Arm 1 togoal: 25.2513808714623
Mobile Platform 2 Pose: [2.16116569392385;7.1497107854615;-16.0701535992009]
Robot Arm 2 end: [7.82144091010595;-15.812128132125;0.575910596539992]
Robot Arm 1 togoal: 18.7690645349514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-0.712777297105364;-8.74523958240281;26.5649679090322]
Robot Arm 1 end: [-8.47115134280512;27.2377468621681;0.799583743535134]
Robot Arm 1 togoal: 25.2753593491386
Mobile Platform 2 Pose: [2.16169611504071;7.15074031494579;-16.2800843004157]
Robot Arm 2 end: [7.83081078176065;-16.0147335269308;0.592293872699428]
Robot Arm 1 togoal: 18.8408420089396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999662e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-0.635614785601632;-8.78382324614319;26.5610941930384]
Robot Arm 1 end: [-8.50997915111999;27.2337782070136;0.801179820459195]
Robot Arm 1 togoal: 25.3009995959509
Mobile Platform 2 Pose: [2.16233901221986;7.15169355750178;-16.4970505655446]
Robot Arm 2 end: [7.83743973557035;-16.2282594903356;0.600023895714439]
Robot Arm 1 togoal: 18.9159633533089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999847e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-0.554324257847244;-8.82446943034538;26.5570666191074]
Robot Arm 1 end: [-8.5509315232339;27.2296894364039;0.802682573690551]
Robot Arm 1 togoal: 25.3281511046421
Mobile Platform 2 Pose: [2.16306448453002;7.15259157821226;-16.7190712804696]
Robot Arm 2 end: [7.84227571584498;-16.44909139041;0.601154106156355]
Robot Arm 1 togoal: 18.9943070142217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000002e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [-0.469287100148608;-8.86698761869621;26.5529194307277]
Robot Arm 1 end: [-8.59381212357096;27.2255115697348;0.804101667539755]
Robot Arm 1 togoal: 25.356696183549
Mobile Platform 2 Pose: [2.16052874393068;7.15511099578049;-16.9446624773175]
Robot Arm 2 end: [7.85123940993237;-16.6774832113758;0.620545980160638]
Robot Arm 1 togoal: 19.0805227378805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000015e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [-0.380818634282425;-8.91122032386488;26.5486825645975]
Robot Arm 1 end: [-8.63845857841571;27.2212717394195;0.805445676761755]
Robot Arm 1 togoal: 25.3865388836633
Mobile Platform 2 Pose: [2.16326794230029;7.15501882141594;-17.1707284338711]
Robot Arm 2 end: [7.83964039917947;-16.8988356238507;0.591846357631658]
Robot Arm 1 togoal: 19.1496743843877

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000003e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [-0.289185198649572;-8.95703450364457;26.5443823378857]
Robot Arm 1 end: [-8.6847336724006;27.2169937942804;0.806722214079571]
Robot Arm 1 togoal: 25.41759903052
Mobile Platform 2 Pose: [2.1605634611916;7.15766507182608;-17.3917953654673]
Robot Arm 2 end: [7.84938937027128;-17.1243921099978;0.60874676611706]
Robot Arm 1 togoal: 19.2403188588776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000049e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [-0.194620313845017;-9.00431352982751;26.5400413642626]
Robot Arm 1 end: [-8.73251711118141;27.2126981347734;0.807938040920263]
Robot Arm 1 togoal: 25.4498061241539
Mobile Platform 2 Pose: [2.16185004503193;7.15833560678956;-17.6139582045728]
Robot Arm 2 end: [7.84126543246079;-17.3460850407383;0.582417693279621]
Robot Arm 1 togoal: 19.3172631188729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000003e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [-0.09733953538808;-9.05294976497985;26.5356803811545]
Robot Arm 1 end: [-8.78169791025567;27.2084034661125;0.809099159277465]
Robot Arm 1 togoal: 25.4830949661072
Mobile Platform 2 Pose: [2.15893774649873;7.16114343363816;-17.8301410818559]
Robot Arm 2 end: [7.85171671363439;-17.5677003931956;0.596065787958131]
Robot Arm 1 togoal: 19.4115785592422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000067e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [0.00244408711983826;-9.10283675534672;26.5313162696307]
Robot Arm 1 end: [-8.83216639396349;27.2041247403553;0.810210895753316]
Robot Arm 1 togoal: 25.5173983846777
Mobile Platform 2 Pose: [2.15897334108521;7.16248987996695;-18.0480447383098]
Robot Arm 2 end: [7.84409585879262;-17.7866583237805;0.569626816562498]
Robot Arm 1 togoal: 19.4922783635788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000064e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [0.104517766108711;-9.15386822552279;26.5269639107688]
Robot Arm 1 end: [-8.88381315476764;27.199875000685;0.811277961508301]
Robot Arm 1 togoal: 25.5526477038151
Mobile Platform 2 Pose: [2.15869484175533;7.16404194361954;-18.2590370899545]
Robot Arm 2 end: [7.83901294647287;-18.0008490242177;0.5473105720383]
Robot Arm 1 togoal: 19.5752915461016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000003e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [0.208746586482497;-9.20597678519296;26.5226377497625]
Robot Arm 1 end: [-8.93656866252745;27.1956673069537;0.812304513183087]
Robot Arm 1 togoal: 25.5888033353474
Mobile Platform 2 Pose: [2.15531143959419;7.1671909179979;-18.4647430981674]
Robot Arm 2 end: [7.8506357783953;-18.2133413353881;0.555562466468607]
Robot Arm 1 togoal: 19.6732367244354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000080e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [0.208634918051473;-9.20591467090198;26.518348629414]
Robot Arm 1 end: [-8.93593391899572;27.1910111672282;0.813294255292734]
Robot Arm 1 togoal: 25.58517746191
Mobile Platform 2 Pose: [2.15343698642076;7.16958830741457;-18.672901949239]
Robot Arm 2 end: [7.84519813317765;-18.4250127082138;0.530281968885598]
Robot Arm 1 togoal: 19.7588830089939

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000084e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [0.139257876887218;-9.17121846060978;26.5141587094313]
Robot Arm 1 end: [-8.89986579203011;27.1861427985283;0.814197280330545]
Robot Arm 1 togoal: 25.5551951926157
Mobile Platform 2 Pose: [2.1512782095957;7.17217324950506;-18.8750730556369]
Robot Arm 2 end: [7.83989303492939;-18.6320905767679;0.504973081676878]
Robot Arm 1 togoal: 19.8445731100975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996799e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000091e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [0.103724824978455;-9.15344236562378;26.5101357178366]
Robot Arm 1 end: [-8.7888268321531;27.1627247222666;0.652070512166441]
Robot Arm 1 togoal: 25.4624070361732
Mobile Platform 2 Pose: [2.14880826539905;7.17495911045536;-19.07174033516]
Robot Arm 2 end: [7.83399280668259;-18.8346443006322;0.478583746464834]
Robot Arm 1 togoal: 19.9295978633071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [0.0637785964425515;-9.1325526818859;26.5197989079983]
Robot Arm 1 end: [-8.74174054287139;27.1529347073614;0.508659727429618]
Robot Arm 1 togoal: 25.4263807978796
Mobile Platform 2 Pose: [2.15172109573075;7.17509997803725;-19.2630943962615]
Robot Arm 2 end: [7.85765809299511;-19.0259334445253;0.581944433680277]
Robot Arm 1 togoal: 20.0311071203753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991934e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [0.00527281186465101;-9.10209524606377;26.5544835625355]
Robot Arm 1 end: [-8.68634466542765;27.135165421644;0.37139439252262]
Robot Arm 1 togoal: 25.379420088627
Mobile Platform 2 Pose: [2.16798676771207;7.16860489305785;-19.4604974127913]
Robot Arm 2 end: [7.82860281313076;-19.1918276692599;0.625116618421758]
Robot Arm 1 togoal: 20.0786299633001

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998636e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999901e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [-0.0762840982836756;-9.0596676557197;26.6022652660657]
Robot Arm 1 end: [-8.63560837883943;27.1211369642951;0.245264521341343]
Robot Arm 1 togoal: 25.3386410766468
Mobile Platform 2 Pose: [2.17452901640196;7.16693649459087;-19.6471673257423]
Robot Arm 2 end: [7.81201200476178;-19.3796976493506;0.671402750858726]
Robot Arm 1 togoal: 20.1486554175895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [-0.173410097672247;-9.00915217328961;26.6508698389859]
Robot Arm 1 end: [-8.59269924314414;27.1031496362535;0.138486617120921]
Robot Arm 1 togoal: 25.300544181498
Mobile Platform 2 Pose: [2.18868219376879;7.16162695791529;-19.8194088531164]
Robot Arm 2 end: [7.75449423833966;-19.5331092878851;0.686385194323426]
Robot Arm 1 togoal: 20.1692543299908

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [-0.215666669830125;-8.98582270504553;26.6958818299097]
Robot Arm 1 end: [-8.58852282095371;27.1061942787297;0.0960868614723698]
Robot Arm 1 togoal: 25.3018861118912
Mobile Platform 2 Pose: [2.20866052847252;7.15353073911801;-19.9572617883077]
Robot Arm 2 end: [7.67571844712328;-19.6469968821326;0.705608268594655]
Robot Arm 1 togoal: 20.1534224175819

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [-0.253625062914925;-8.96492223763906;26.749246409421]
Robot Arm 1 end: [-8.56313491235717;27.145780901495;0.0980978099774073]
Robot Arm 1 togoal: 25.3099275214058
Mobile Platform 2 Pose: [2.23348930865095;7.14304772527667;-20.053359804558]
Robot Arm 2 end: [7.58644227172799;-19.7235931911436;0.727219479539081]
Robot Arm 1 togoal: 20.1112233302288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998948e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [-0.254046569028374;-8.96307519132183;26.799238510359]
Robot Arm 1 end: [-8.53809044732747;27.2014492848952;0.132894449271737]
Robot Arm 1 togoal: 25.3274059460119
Mobile Platform 2 Pose: [2.26288073122898;7.13022182044152;-20.1069988858783]
Robot Arm 2 end: [7.49165482159489;-19.7646574799856;0.745166512449367]
Robot Arm 1 togoal: 20.0476542692572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [-0.255759618334289;-8.96078982145643;26.8365955507625]
Robot Arm 1 end: [-8.54794889668026;27.2187718653461;0.118515313826296]
Robot Arm 1 togoal: 25.3471788591877
Mobile Platform 2 Pose: [2.2966517333927;7.11503179303563;-20.1202805535211]
Robot Arm 2 end: [7.3975679387772;-19.7729802671388;0.756366359509196]
Robot Arm 1 togoal: 19.9692558221488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999800e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [-0.261045755342049;-8.95672192217943;26.8644942770815]
Robot Arm 1 end: [-8.56990061540198;27.2276212718936;0.0984723661929369]
Robot Arm 1 togoal: 25.3703710738068
Mobile Platform 2 Pose: [2.33467489751395;7.09743673242616;-20.0976023126072]
Robot Arm 2 end: [7.3086844926948;-19.7531074453054;0.75974224976178]
Robot Arm 1 togoal: 19.8819812159056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [-0.270125728960898;-8.95078926643396;26.8794749671357]
Robot Arm 1 end: [-8.60211626289859;27.2078312134231;0.0739559470496994]
Robot Arm 1 togoal: 25.381925084974
Mobile Platform 2 Pose: [2.37675438468077;7.07745755213869;-20.0440519810716]
Robot Arm 2 end: [7.2273402754273;-19.7101216620029;0.756300643237608]
Robot Arm 1 togoal: 19.7902117412242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [-0.283527307179228;-8.942586844062;26.8772440009437]
Robot Arm 1 end: [-8.64732596988986;27.1601771587519;0.0540189269802664]
Robot Arm 1 togoal: 25.3839807637745
Mobile Platform 2 Pose: [2.42274793336657;7.05512158019103;-19.9648918261008]
Robot Arm 2 end: [7.15662544650979;-19.6424115296592;0.757425036798498]
Robot Arm 1 togoal: 19.6954041223723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [-0.300883085919698;-8.93222494291197;26.858781282762]
Robot Arm 1 end: [-8.69967586018593;27.0991572724693;0.0472034158558706]
Robot Arm 1 togoal: 25.3817086319282
Mobile Platform 2 Pose: [2.47371407282176;7.029915662902;-19.8672433610245]
Robot Arm 2 end: [7.09831802169326;-19.5425350570134;0.780931945431411]
Robot Arm 1 togoal: 19.5949012554461

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999001e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [-0.321161097293096;-8.92023769610622;26.8303082148783]
Robot Arm 1 end: [-8.7517109589036;27.0382483908713;0.0535540990318636]
Robot Arm 1 togoal: 25.3787700566352
Mobile Platform 2 Pose: [2.53268513868839;7.00039491177739;-19.760670391598]
Robot Arm 2 end: [7.02470491534627;-19.4391830300665;0.809107443419093]
Robot Arm 1 togoal: 19.479362020914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000656e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [-0.343388339062167;-8.90713333376698;26.8012439775064]
Robot Arm 1 end: [-8.80285617948815;26.9890196904564;0.068054838240892]
Robot Arm 1 togoal: 25.3827744778286
Mobile Platform 2 Pose: [2.60003353815482;6.96626966304981;-19.6481928759226]
Robot Arm 2 end: [6.92474492283924;-19.3399786620161;0.838623828519196]
Robot Arm 1 togoal: 19.3429012650505

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000984e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [-0.366708495347397;-8.89336838221983;26.7824748236439]
Robot Arm 1 end: [-8.85827667034684;26.96072662637;0.0858571915069167]
Robot Arm 1 togoal: 25.4039461806738
Mobile Platform 2 Pose: [2.67517993352587;6.92765451608042;-19.5328630207071]
Robot Arm 2 end: [6.81526062891039;-19.2588314840023;0.873864115499753]
Robot Arm 1 togoal: 19.2067106845588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000781e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [-0.390329573945447;-8.879382982693;26.7861640195366]
Robot Arm 1 end: [-8.92096460681815;26.9581984033316;0.104403100318371]
Robot Arm 1 togoal: 25.4477924684129
Mobile Platform 2 Pose: [2.75761381693147;6.8845376542699;-19.4274977499494]
Robot Arm 2 end: [6.73249325467413;-19.2063731099112;0.92659539658095]
Robot Arm 1 togoal: 19.107248707995

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000506e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [-0.413657867534032;-8.86550010697584;26.8244999641399]
Robot Arm 1 end: [-8.9835243871605;26.9801820556459;0.124654823494251]
Robot Arm 1 togoal: 25.5078529212622
Mobile Platform 2 Pose: [2.82006037840151;6.8503955384551;-19.3609800870787]
Robot Arm 2 end: [6.65943211002654;-19.2071552074143;0.969934367591642]
Robot Arm 1 togoal: 19.0423753395742

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000277e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [-0.436495901812681;-8.85178917466344;26.9022013516176]
Robot Arm 1 end: [-9.04018699166078;27.0276492944191;0.148585999855724]
Robot Arm 1 togoal: 25.5803260604499
Mobile Platform 2 Pose: [2.87399280554494;6.81945913532533;-19.3499971195003]
Robot Arm 2 end: [6.68599568460166;-19.218390657699;1.0287853632592]
Robot Arm 1 togoal: 19.0694376831914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000046e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974625e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [-0.458660368431411;-8.83834087742756;27.0154349089839]
Robot Arm 1 end: [-9.08539649133873;27.0987545688445;0.174205489285138]
Robot Arm 1 togoal: 25.6599614670653
Mobile Platform 2 Pose: [2.93767213347366;6.78309453805082;-19.3968332494792]
Robot Arm 2 end: [6.69130474622528;-19.3129945372888;1.07330094390375]
Robot Arm 1 togoal: 19.1188900260387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000183e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000799e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [-0.488206366426453;-8.82111699081;27.1584710993435]
Robot Arm 1 end: [-9.10473005036862;27.2057239091829;0.219866691633203]
Robot Arm 1 togoal: 25.7433601812705
Mobile Platform 2 Pose: [2.9320557728024;6.78068929092779;-19.5044694446837]
Robot Arm 2 end: [6.78681070061297;-19.4356178007869;1.10054461826803]
Robot Arm 1 togoal: 19.2614817578702

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000745e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [-0.51232636301084;-8.80655841272911;27.2651097516308]
Robot Arm 1 end: [-9.1287576477539;27.3487402271718;0.252645661857286]
Robot Arm 1 togoal: 25.8553065992936
Mobile Platform 2 Pose: [2.92810964858921;6.7770917104898;-19.658478241958]
Robot Arm 2 end: [6.89156433777335;-19.6139133714849;1.0992506765968]
Robot Arm 1 togoal: 19.4404256276282

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000424e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [-0.525228883567283;-8.7983144680675;27.354502157859]
Robot Arm 1 end: [-9.13823237415983;27.4729661607351;0.274816424586179]
Robot Arm 1 togoal: 25.9447385487918
Mobile Platform 2 Pose: [2.92651609874403;6.77218125232126;-19.8503495878649]
Robot Arm 2 end: [6.98948722873055;-19.856795844776;1.0670726220061]
Robot Arm 1 togoal: 19.6470137854404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992052e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [-0.52911311028806;-8.79507687486628;27.4415157772437]
Robot Arm 1 end: [-9.13366237384573;27.5307955530039;0.301505411662023]
Robot Arm 1 togoal: 25.9790876592795
Mobile Platform 2 Pose: [2.9207815444415;6.76956887565491;-20.0656431001473]
Robot Arm 2 end: [7.05919360323268;-20.1624791297306;1.00620828953568]
Robot Arm 1 togoal: 19.8634609008588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [-0.530053586033896;-8.79286759569399;27.5225469385859]
Robot Arm 1 end: [-9.12743748422124;27.5915333533043;0.313930352582227]
Robot Arm 1 togoal: 26.0149558440318
Mobile Platform 2 Pose: [2.90904984170068;6.77078494520216;-20.2742884220142]
Robot Arm 2 end: [7.08685295086402;-20.4729249822714;0.923024351401451]
Robot Arm 1 togoal: 20.0499445466827

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000033e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [-0.528224987975729;-8.79179229778079;27.5952257570484]
Robot Arm 1 end: [-9.11982632043893;27.6443508745269;0.316379667712383]
Robot Arm 1 togoal: 26.0449992599756
Mobile Platform 2 Pose: [2.88908292908641;6.77745172662464;-20.4250728059909]
Robot Arm 2 end: [7.07943728803853;-20.719795553184;0.825948942066675]
Robot Arm 1 togoal: 20.1768255865793

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000022e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [-0.521828611965383;-8.79296515017654;27.6532426238995]
Robot Arm 1 end: [-9.09854080030376;27.656247426946;0.313245721873831]
Robot Arm 1 togoal: 26.0375882965135
Mobile Platform 2 Pose: [2.845476373106;6.79754186719701;-20.4847631812043]
Robot Arm 2 end: [7.0548922458885;-20.859846277509;0.797775310496791]
Robot Arm 1 togoal: 20.2318724946926

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [-0.513947050688319;-8.79435471495182;27.6954377498947]
Robot Arm 1 end: [-9.06473547226078;27.6457543851774;0.302019953846817]
Robot Arm 1 togoal: 26.0062050057713
Mobile Platform 2 Pose: [2.79371464275282;6.82299834361913;-20.5074269044363]
Robot Arm 2 end: [7.02431807024878;-20.9748737637267;0.774152418899268]
Robot Arm 1 togoal: 20.2690167792158

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995908e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [-0.503087790644545;-8.79683814233639;27.7201540271896]
Robot Arm 1 end: [-9.01926942543798;27.6133141081589;0.281832026822008]
Robot Arm 1 togoal: 25.9518203719088
Mobile Platform 2 Pose: [2.73608709394863;6.85239276529301;-20.5136676590859]
Robot Arm 2 end: [6.97083436355154;-21.0648955169198;0.74732972539318]
Robot Arm 1 togoal: 20.2741139754433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997582e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [-0.485440476499565;-8.80217234707314;27.7339207991486]
Robot Arm 1 end: [-9.0072026963236;27.6154127842072;0.268389386482981]
Robot Arm 1 togoal: 25.9450487616855
Mobile Platform 2 Pose: [2.74239423354052;6.85048710312772;-20.5211256125185]
Robot Arm 2 end: [6.86158367834902;-21.1389046354878;0.714579199478123]
Robot Arm 1 togoal: 20.2247006189949

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987519e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998906e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [-0.470917641916543;-8.80529605569157;27.7671061333897]
Robot Arm 1 end: [-8.91244439460432;27.5827982559362;0.254907345827976]
Robot Arm 1 togoal: 25.8541744810563
Mobile Platform 2 Pose: [2.75572808637784;6.84548237382126;-20.5353214407791]
Robot Arm 2 end: [6.74908263811625;-21.1895827142973;0.696621058591281]
Robot Arm 1 togoal: 20.1598147604978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [-0.455778880145888;-8.80810562926326;27.7969170634462]
Robot Arm 1 end: [-8.82125919112891;27.5785673356414;0.26024673562752]
Robot Arm 1 togoal: 25.7844008250523
Mobile Platform 2 Pose: [2.77603263961671;6.83722339174617;-20.5616871878388]
Robot Arm 2 end: [6.64035947721089;-21.2317197373586;0.684226294054735]
Robot Arm 1 togoal: 20.0936129659236

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000126e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [-0.440628616276527;-8.81054959382973;27.835196624615]
Robot Arm 1 end: [-8.7274908177632;27.6183277262072;0.259470154320135]
Robot Arm 1 togoal: 25.7432678918001
Mobile Platform 2 Pose: [2.71502550355413;6.86973657082516;-20.5978049147591]
Robot Arm 2 end: [6.52965702439866;-21.2567798699417;0.61072519876381]
Robot Arm 1 togoal: 20.0193220314361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000167e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [-0.425833211974895;-8.81266063282579;27.916479846312]
Robot Arm 1 end: [-8.63384997691847;27.7243748380515;0.260511193451252]
Robot Arm 1 togoal: 25.7480612938817
Mobile Platform 2 Pose: [2.72412718636;6.86728247326908;-20.631402970645]
Robot Arm 2 end: [6.43032373763141;-21.2712455773242;0.592815347642446]
Robot Arm 1 togoal: 19.9463791635624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [-0.407848825141084;-8.81651173062514;28.0564831859472]
Robot Arm 1 end: [-8.56183215826924;27.8981004749527;0.27055383702787]
Robot Arm 1 togoal: 25.815639238449
Mobile Platform 2 Pose: [2.61429497287383;6.92433146869449;-20.6717150781121]
Robot Arm 2 end: [6.4914276827187;-21.318044711829;0.736247360458868]
Robot Arm 1 togoal: 20.0170217519632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984535e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [-0.463907206802478;-8.78358487537019;28.2461355149942]
Robot Arm 1 end: [-8.58869019293943;28.0148879143669;0.26436994993671]
Robot Arm 1 togoal: 25.9163019881118
Mobile Platform 2 Pose: [2.51460192216352;6.97630827879972;-20.7444990809581]
Robot Arm 2 end: [6.56458530394033;-21.3744735948649;0.878775878874946]
Robot Arm 1 togoal: 20.1041435448902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988662e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [-0.516172191326495;-8.75246175125603;28.3611164352021]
Robot Arm 1 end: [-8.63883575711219;28.0747446919727;0.232325301623515]
Robot Arm 1 togoal: 25.9954472271364
Mobile Platform 2 Pose: [2.42964747752912;7.0208480290954;-20.8566632106987]
Robot Arm 2 end: [6.64536388037251;-21.4491963863882;1.01021385198297]
Robot Arm 1 togoal: 20.2092340056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993843e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [-0.562599428120762;-8.72459529427366;28.4125398944835]
Robot Arm 1 end: [-8.71371177175368;28.1024177004151;0.189297193569393]
Robot Arm 1 togoal: 26.070557478528
Mobile Platform 2 Pose: [2.35050115098167;7.0623876930166;-21.0091608563185]
Robot Arm 2 end: [6.7346388806333;-21.5473534575673;1.12447323346721]
Robot Arm 1 togoal: 20.3358487360565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999279e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [-0.601876007521573;-8.70095955757304;28.4175812699476]
Robot Arm 1 end: [-8.80308297220665;28.1292880541945;0.14931537339558]
Robot Arm 1 togoal: 26.1554685166699
Mobile Platform 2 Pose: [2.29489292476014;7.09206752103977;-21.1980088798287]
Robot Arm 2 end: [6.82041356430897;-21.6693446228038;1.21943386025859]
Robot Arm 1 togoal: 20.4741509845167

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000415e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [-0.629647571469385;-8.68407962379605;28.4003872839652]
Robot Arm 1 end: [-8.89474563645074;28.1781971246954;0.132023416161512]
Robot Arm 1 togoal: 26.2561901143504
Mobile Platform 2 Pose: [2.28069068864054;7.10097839255058;-21.4166041524169]
Robot Arm 2 end: [6.89165955023491;-21.8118875498324;1.29511183337994]
Robot Arm 1 togoal: 20.6130996047355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000232e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [-0.645131942620631;-8.67408670711561;28.3811754872906]
Robot Arm 1 end: [-8.92219052159339;28.1750883894279;0.137417024084604]
Robot Arm 1 togoal: 26.2735120216205
Mobile Platform 2 Pose: [2.27084237245529;7.10767960862014;-21.657277392828]
Robot Arm 2 end: [6.96540926598962;-21.9693099265312;1.35205492979528]
Robot Arm 1 togoal: 20.7633820490938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000083e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [-0.662845828769733;-8.66310471655815;28.3590540236626]
Robot Arm 1 end: [-8.95709644723326;28.1826920228036;0.159809655509987]
Robot Arm 1 togoal: 26.3027495532266
Mobile Platform 2 Pose: [2.26990437882062;7.10988272359104;-21.9093800019361]
Robot Arm 2 end: [7.03906226862101;-22.1323432588431;1.3904702925377]
Robot Arm 1 togoal: 20.9174671972701

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [-0.682465687955402;-8.6508599251871;28.3664767871829]
Robot Arm 1 end: [-9.00456265394235;28.2146844037829;0.194675505350396]
Robot Arm 1 togoal: 26.3573000563817
Mobile Platform 2 Pose: [2.26736953756397;7.11297300384686;-22.162154298332]
Robot Arm 2 end: [7.11726489308373;-22.2907249944336;1.40974378107596]
Robot Arm 1 togoal: 21.0730884365591

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992338e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [-0.701972473479884;-8.63867225357865;28.3994653485056]
Robot Arm 1 end: [-9.04501519551462;28.2381536905918;0.236509187783391]
Robot Arm 1 togoal: 26.4006206458428
Mobile Platform 2 Pose: [2.28919942516647;7.10395575404691;-22.4139520739817]
Robot Arm 2 end: [7.18566731513405;-22.4431540088203;1.4082720411258]
Robot Arm 1 togoal: 21.2177272662837

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988488e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [-0.720328176719155;-8.62693246004145;28.4510224895614]
Robot Arm 1 end: [-9.08270263320359;28.2670107033541;0.283983073079682]
Robot Arm 1 togoal: 26.445480345443
Mobile Platform 2 Pose: [2.3369753170132;7.08209587666745;-22.651617600083]
Robot Arm 2 end: [7.24210209571059;-22.5788024755779;1.38360076770482]
Robot Arm 1 togoal: 21.3432714209314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [-0.738197433944586;-8.61528625828192;28.5200845994529]
Robot Arm 1 end: [-9.11451642055469;28.3020952898901;0.335729370390238]
Robot Arm 1 togoal: 26.4903057071316
Mobile Platform 2 Pose: [2.35038132266984;7.07755334144319;-22.8650903177555]
Robot Arm 2 end: [7.30609902116558;-22.6084099671196;1.26878775805781]
Robot Arm 1 togoal: 21.4134695050647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [-0.754049938505164;-8.60455716804394;28.6020444046136]
Robot Arm 1 end: [-9.08582313690503;28.3540496679753;0.316283396342546]
Robot Arm 1 togoal: 26.5064355604983
Mobile Platform 2 Pose: [2.37837239891525;7.06573935370916;-23.0139009213856]
Robot Arm 2 end: [7.27497998596755;-22.5988777928135;1.11327892387951]
Robot Arm 1 togoal: 21.3851091096057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000036e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [-0.769461163630339;-8.59413175650803;28.6823439453076]
Robot Arm 1 end: [-9.14093803190891;28.4546068571075;0.385031227767966]
Robot Arm 1 togoal: 26.6127497272128
Mobile Platform 2 Pose: [2.41313359520893;7.05030611465297;-23.0788063581258]
Robot Arm 2 end: [7.18119834057261;-22.5654287144797;0.942700885203057]
Robot Arm 1 togoal: 21.2936141850777

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [-0.783949634355345;-8.58420053290286;28.7814966198729]
Robot Arm 1 end: [-9.14462634362415;28.5562538575445;0.400395337272266]
Robot Arm 1 togoal: 26.6853336099205
Mobile Platform 2 Pose: [2.44290820103068;7.03690049597776;-23.0523946323385]
Robot Arm 2 end: [7.06444819983189;-22.496454135241;0.843120438785359]
Robot Arm 1 togoal: 21.1616700432277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997702e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [-0.798877852039573;-8.57390175717255;28.8879281907571]
Robot Arm 1 end: [-9.13345636704679;28.6636867036755;0.390275654811777]
Robot Arm 1 togoal: 26.7525866313416
Mobile Platform 2 Pose: [2.4895415433999;7.01446062574005;-22.9706803011861]
Robot Arm 2 end: [6.93490222046882;-22.4095700621489;0.877236447137523]
Robot Arm 1 togoal: 21.0048273941068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997730e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [-0.813599934230319;-8.56368101612019;28.9978969392149]
Robot Arm 1 end: [-9.12243897814355;28.774860591717;0.381186129059403]
Robot Arm 1 togoal: 26.8227932619185
Mobile Platform 2 Pose: [2.53950270272483;6.9898202590421;-22.8824726168789]
Robot Arm 2 end: [6.80163038534763;-22.339327320908;0.892707958262788]
Robot Arm 1 togoal: 20.8554197566976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997839e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [-0.828100991238681;-8.55355071401133;29.1109895084452]
Robot Arm 1 end: [-9.11214476356904;28.8894647624365;0.374101484421731]
Robot Arm 1 togoal: 26.8961211456222
Mobile Platform 2 Pose: [2.59760784201052;6.96037654754623;-22.7918643640381]
Robot Arm 2 end: [6.67093652351933;-22.2876536916864;0.837573415029228]
Robot Arm 1 togoal: 20.7213327205339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997926e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [-0.84231871933968;-8.54355486125462;29.2263442784116]
Robot Arm 1 end: [-9.10245394502706;29.0064844579764;0.368545847423617]
Robot Arm 1 togoal: 26.9718065532691
Mobile Platform 2 Pose: [2.67077471438735;6.92257619417854;-22.6865630816218]
Robot Arm 2 end: [6.54798321114115;-22.2425548629984;0.864250754264022]
Robot Arm 1 togoal: 20.5948347286768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997994e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [-0.856211491006937;-8.53372339549533;29.3432702570221]
Robot Arm 1 end: [-9.09327331639857;29.1251417071149;0.364165624779319]
Robot Arm 1 togoal: 27.0492587148634
Mobile Platform 2 Pose: [2.72787651650469;6.89189589620022;-22.6094970510073]
Robot Arm 2 end: [6.46261308830985;-22.2339897491305;0.860865238264088]
Robot Arm 1 togoal: 20.5213038762373

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998048e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [-0.869751655465156;-8.52407680162371;29.4612146700235]
Robot Arm 1 end: [-9.08452572746458;29.2448307342214;0.360694936764212]
Robot Arm 1 togoal: 27.1280102490186
Mobile Platform 2 Pose: [2.78257774540696;6.86157696311244;-22.558665147592]
Robot Arm 2 end: [6.40785845622642;-22.2645399761683;0.945367361937125]
Robot Arm 1 togoal: 20.4931235510987

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [-0.8829218125189;-8.51462841916514;29.579734651809]
Robot Arm 1 end: [-9.076150596397;29.3650786971961;0.357929841167969]
Robot Arm 1 togoal: 27.2076924181266
Mobile Platform 2 Pose: [2.84443183123552;6.82669661396605;-22.577310137725]
Robot Arm 2 end: [6.39746827533905;-22.3741707725131;1.00106455091966]
Robot Arm 1 togoal: 20.5491459924562

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mtimes')" style="font-weight:bold"> * </a>
用于矩阵乘法的维度不正确。请检查并确保第一个矩阵中的列数与第二个矩阵中的行数匹配。要单独对矩阵的每个元素进行运算，请使用 TIMES (.*)执行按
元素相乘。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 19)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',19,0)">第 19 行</a>)
    X_d = pinv(W*mobile_robot.J)*W*s_k;

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
    [dx1] = mobile_robot_CBLF(mobile_robot1, target1, a1_G, obstacle_results1, D_min, V_max, dt);

<a href="matlab:helpview('matlab','error_innerdim')" style="font-weight:bold">相关文档</a>
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',19,0)
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',19,1)">19  </a>    X_d = pinv(W*mobile_robot.J)*W*s_k;
W

W =

    0.1000         0         0         0         0         0         0         0         0         0         0
         0    0.1000         0         0         0         0         0         0         0         0         0
         0         0    0.1000         0         0         0         0         0         0         0         0
         0         0         0    0.1000         0         0         0         0         0         0         0
         0         0         0         0    2.0000         0         0         0         0         0         0
         0         0         0         0         0    2.0000         0         0         0         0         0
         0         0         0         0         0         0    2.0000         0         0         0         0
         0         0         0         0         0         0         0    2.0000         0         0         0
         0         0         0         0         0         0         0         0    2.0000         0         0
         0         0         0         0         0         0         0         0         0    2.0000         0
         0         0         0         0         0         0         0         0         0         0    2.0000

W*mobile_robot.J
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mtimes')" style="font-weight:bold"> * </a>
用于矩阵乘法的维度不正确。请检查并确保第一个矩阵中的列数与第二个矩阵中的行数匹配。要单独对矩阵的每个元素进行运算，请使用 TIMES (.*)执行按
元素相乘。

<a href="matlab:helpview('matlab','error_innerdim')" style="font-weight:bold">相关文档</a>
} 
W'*mobile_robot.J
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mtimes')" style="font-weight:bold"> * </a>
用于矩阵乘法的维度不正确。请检查并确保第一个矩阵中的列数与第二个矩阵中的行数匹配。要单独对矩阵的每个元素进行运算，请使用 TIMES (.*)执行按
元素相乘。

<a href="matlab:helpview('matlab','error_innerdim')" style="font-weight:bold">相关文档</a>
} 
mobile_robot.J

ans =

         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0    1.0000         0   -1.0000         0    1.0000         0
   -0.1179    0.1179   -0.1179    0.1179    1.0000         0    1.0000         0    1.0000         0    1.0000
    0.0943   -0.0236    0.0236   -0.0943         0   -1.5610         0    1.9610         0   -2.3610         0
    0.0354   -0.0354   -0.0354    0.0354         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',19,1)">19  </a>    X_d = pinv(W*mobile_robot.J)*W*s_k;
mobile_robot.J

ans =

         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0    1.0000         0   -1.0000         0    1.0000         0
   -0.1179    0.1179   -0.1179    0.1179    1.0000         0    1.0000         0    1.0000         0    1.0000
    0.0943   -0.0236    0.0236   -0.0943         0   -1.5610         0    1.9610         0   -2.3610         0
    0.0354   -0.0354   -0.0354    0.0354         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',19,1)">19  </a>    X_d = pinv(W'*mobile_robot.J)*W'*s_k;
if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
{错误使用 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mtimes')" style="font-weight:bold"> * </a>
用于矩阵乘法的维度不正确。请检查并确保第一个矩阵中的列数与第二个矩阵中的行数匹配。要单独对矩阵的每个元素进行运算，请使用 TIMES (.*)执行按
元素相乘。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 19)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',19,0)">第 19 行</a>)
    X_d = pinv(W'*mobile_robot.J)*W'*s_k;

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 97)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',97,0)">第 97 行</a>)
    [dx1] = mobile_robot_CBLF(mobile_robot1, target1, a1_G, obstacle_results1, D_min, V_max, dt);

<a href="matlab:helpview('matlab','error_innerdim')" style="font-weight:bold">相关文档</a>
} 
opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',19,0)
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000998e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.35261120707987;9.99567223264587;9.81851213436114]
Robot Arm 2 end: [9.77363252834204;9.83022755411435;1.48915678582505]
Robot Arm 1 togoal: 28.0042047236509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994674e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.15548114244931;9.86199060135234;9.81489131896234]
Robot Arm 2 end: [9.54823427383523;9.88360247006103;1.45993795398377]
Robot Arm 1 togoal: 27.8835944416359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883460e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-2.00123250369193;9.74654284731324;9.82813715601991]
Robot Arm 2 end: [9.24968821472064;9.99363518766632;1.34346539909181]
Robot Arm 1 togoal: 27.7547736959962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.878245e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.89661199654805;9.65423145184345;9.85236038496748]
Robot Arm 2 end: [9.00419654356273;9.99252694462811;1.20696992705993]
Robot Arm 1 togoal: 27.5854895694231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.75757171527408;9.52858445452878;9.87448461460346]
Robot Arm 2 end: [8.81470599066007;10.0361702358813;1.12787390619007]
Robot Arm 1 togoal: 27.4881033787679

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820607e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.592023044267;9.4474295479522;9.86648766276224]
Robot Arm 2 end: [8.68002273976466;9.9982967289359;1.0419051418822]
Robot Arm 1 togoal: 27.3697762778538

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880274e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.50741513865487;9.39998071874783;9.86180168798259]
Robot Arm 2 end: [8.63522995146617;9.83413375105117;1.06112626539777]
Robot Arm 1 togoal: 27.2190400427786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.44463610010666;9.36250322773659;9.85449628658047]
Robot Arm 2 end: [8.65139680482276;9.69873387466789;1.10650952596554]
Robot Arm 1 togoal: 27.1309144959953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.953881e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.38333623325871;9.32712030726989;9.84402248357454]
Robot Arm 2 end: [8.66460499105644;9.60132186934604;1.11105943817343]
Robot Arm 1 togoal: 27.0693064145002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.32300331743275;9.29266956905983;9.82952734636305]
Robot Arm 2 end: [8.63542304730147;9.51067171428656;1.0439854580791]
Robot Arm 1 togoal: 26.9846779815465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.27025792848388;9.26377441470278;9.81578562129871]
Robot Arm 2 end: [8.62912688989954;9.37359423890614;0.968264617512863]
Robot Arm 1 togoal: 26.8828205145369

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.19507052524415;9.22645989749567;9.80278456024319]
Robot Arm 2 end: [8.67260960634503;9.22072152428203;0.891277356453007]
Robot Arm 1 togoal: 26.8048323261453

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.06499287271216;9.16219494653155;9.78104752520379]
Robot Arm 2 end: [8.73921660231523;9.07711343582407;0.808293207052332]
Robot Arm 1 togoal: 26.7507466603177

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.848041217838945;9.04861950852296;9.72784401055777]
Robot Arm 2 end: [8.80292812272293;8.94423246166007;0.721277048114078]
Robot Arm 1 togoal: 26.7034380997929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.672578737205139;8.91433563282294;9.57059599761718]
Robot Arm 2 end: [8.76457747995585;8.76137055635183;0.654985323939137]
Robot Arm 1 togoal: 26.5489806691926

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.915955175991573;8.98407734019459;9.44045612562832]
Robot Arm 2 end: [8.71725387454205;8.64517154389267;0.755693645119022]
Robot Arm 1 togoal: 26.4303558114919

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993870e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-0.891423756864198;8.94137916666386;9.27119302050192]
Robot Arm 2 end: [8.77769492369767;8.44293473645698;0.787049819844272]
Robot Arm 1 togoal: 26.3302554337709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994625e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.07552633782899;9.02346073267869;9.21689820665378]
Robot Arm 2 end: [8.73733957240398;8.44934500072853;0.92311360296891]
Robot Arm 1 togoal: 26.3025415758865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.27013696912434;9.10595957144291;9.10568118668799]
Robot Arm 2 end: [8.75530778695309;8.44017383689636;1.06600281904532]
Robot Arm 1 togoal: 26.30605640823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.3486679348966;9.11215075030265;8.90355207161988]
Robot Arm 2 end: [8.74036322017353;8.25378331296226;1.06091143585793]
Robot Arm 1 togoal: 26.1651199184614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.5193974743218;9.17356003431633;8.73797614362743]
Robot Arm 2 end: [8.81909219945717;8.19073777044591;1.17824159488702]
Robot Arm 1 togoal: 26.1759174656605

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.151434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.39201669517515;9.10386874357142;8.59168332370436]
Robot Arm 2 end: [8.7733943176468;8.01741513666908;1.16307447468171]
Robot Arm 1 togoal: 26.0229839390989
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.29279431293678;9.06328182366241;8.53581581186929]
Robot Arm 2 end: [8.76025212722437;7.89233632798997;1.08924940645065]
Robot Arm 1 togoal: 25.9281539590841
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.08199623543785;8.96418412091856;8.43484091096839]
Robot Arm 2 end: [8.6715717309904;7.73330282458198;1.02599262353805]
Robot Arm 1 togoal: 25.7554392755511
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-0.994436761868591;8.89809791435303;8.24377677378001]
Robot Arm 2 end: [8.63060071561089;7.46407939560359;0.871429319056747]
Robot Arm 1 togoal: 25.5443789165054
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.19392624733099;8.98175076092792;8.14241959999971]
Robot Arm 2 end: [8.65908959384359;7.4712733613372;1.03907000539713]
Robot Arm 1 togoal: 25.566369978105

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','1.794120e-12','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-0.728584947232768;8.7798145727375;7.99503650768881]
Robot Arm 2 end: [8.4273460685023;7.24535497922213;0.856055178454132]
Robot Arm 1 togoal: 25.247009785387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.183596e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-0.261257174117669;8.54574283821263;7.90722743153171]
Robot Arm 2 end: [8.41909402072639;7.16543890039518;1.04955101595827]
Robot Arm 1 togoal: 25.1820885021897

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.773125e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-0.45395244173425;8.64041556649402;7.84805052282664]
Robot Arm 2 end: [8.62273147807774;7.18046662831412;1.14636258203357]
Robot Arm 1 togoal: 25.3399864732826

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-0.373916333406964;8.62287086174181;7.68418593650155]
Robot Arm 2 end: [8.48675511543382;6.94433301485246;1.02305293914025]
Robot Arm 1 togoal: 25.0822344955878

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009655e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-0.577588936213873;8.72092021117092;7.56423341750402]
Robot Arm 2 end: [8.71179307307567;6.91627016619812;1.14964075640642]
Robot Arm 1 togoal: 25.2275504960987

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.879872e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-0.350567291368301;8.62422182807599;7.4516950670931]
Robot Arm 2 end: [8.47127666158861;6.69763606640474;0.973636256397738]
Robot Arm 1 togoal: 24.9057968911428

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010636e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-0.542360414512;8.71184436614344;7.3442438545259]
Robot Arm 2 end: [8.70588131578649;6.61158347422766;1.01795007282237]
Robot Arm 1 togoal: 25.022145402638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008141e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-0.36296699827727;8.63269092138322;7.19749409838974]
Robot Arm 2 end: [8.45853997447531;6.39852318129427;0.825821214326409]
Robot Arm 1 togoal: 24.7004561508649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.872545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-0.576549445067343;8.72849083448265;7.10680913863409]
Robot Arm 2 end: [8.62838689369684;6.36107694334278;0.976456882144241]
Robot Arm 1 togoal: 24.7991565222377

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002231e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-0.401754712165471;8.63400706522456;6.94673419175394]
Robot Arm 2 end: [8.35069271956833;6.17085318054422;0.783344940620536]
Robot Arm 1 togoal: 24.4701563309547

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.792080e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-0.636064617090612;8.72196073548828;6.83616903829555]
Robot Arm 2 end: [8.45700101267205;6.13269459753477;0.976357386869211]
Robot Arm 1 togoal: 24.5198155974947

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008941e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-0.401254673699176;8.57674594597625;6.68235966061392]
Robot Arm 2 end: [8.12325543433444;5.99332574755757;0.786046118429886]
Robot Arm 1 togoal: 24.1822044738203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812255e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-0.588300263066662;8.60254297632421;6.52482284458245]
Robot Arm 2 end: [8.10102879406177;5.93538759490836;0.922460020198888]
Robot Arm 1 togoal: 24.1234755230425

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015583e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-0.295487803006518;8.40284387538114;6.38324513030278]
Robot Arm 2 end: [7.74162193904115;5.87460513294163;0.731627153920704]
Robot Arm 1 togoal: 23.8199778892595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-0.155385374554994;8.22986472869107;6.2027469901439]
Robot Arm 2 end: [7.49754437002747;5.78915662826399;0.637505453641252]
Robot Arm 1 togoal: 23.5847851087598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [0.065827720228047;8.00054690019633;6.04473009545219]
Robot Arm 2 end: [7.33270736005162;5.54678897468125;0.450404213144451]
Robot Arm 1 togoal: 23.308186698911

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.797750e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-0.211687178483037;8.04619980364887;5.93339229229566]
Robot Arm 2 end: [7.31761096926403;5.51335022650898;0.63217867918757]
Robot Arm 1 togoal: 23.2670085802501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.113216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-0.413712505645717;8.00131812277855;5.7849225334383]
Robot Arm 2 end: [7.35948343954292;5.27411035254194;0.744684608808306]
Robot Arm 1 togoal: 23.1355306387382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.425751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-0.568635282726682;7.94644176508544;5.61535303117166]
Robot Arm 2 end: [7.39916699147848;5.04619783471189;0.823434065737754]
Robot Arm 1 togoal: 23.0131631791739

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-0.365682601217008;7.73940685172079;5.44666621117414]
Robot Arm 2 end: [7.06281898568976;4.97092489823026;0.620222118476758]
Robot Arm 1 togoal: 22.7173894811597

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.545203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307233169091;-6.24962959134409;-7.10439671995214]
Robot Arm 1 end: [-6.13859419521266;-6.93244843601462;1.24046716139098]
Robot Arm 1 togoal: 23.3931770247135
Mobile Platform 2 Pose: [-0.659342872993601;7.78876566719633;5.34589979576986]
Robot Arm 2 end: [7.25557472174535;4.76346888005239;0.823223626616646]
Robot Arm 1 togoal: 22.7200740363509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.738760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008906e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407585274638;-6.21793440736385;-7.10337741500991]
Robot Arm 1 end: [-6.10319486646115;-6.88090648515136;1.17441208816718]
Robot Arm 1 togoal: 23.3323383279146
Mobile Platform 2 Pose: [-0.445266545144497;7.58916053223319;5.19537407061862]
Robot Arm 2 end: [7.06351873755362;4.55685340309523;0.616625181952447]
Robot Arm 1 togoal: 22.4473505456115

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.023710e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859461375144;-6.18978152294211;-7.10219117252686]
Robot Arm 1 end: [-6.07854593357295;-6.82857373469496;1.10281726683661]
Robot Arm 1 togoal: 23.2786470844075
Mobile Platform 2 Pose: [-0.720315387704381;7.64857533371012;5.08612827881234]
Robot Arm 2 end: [7.27373640192463;4.37418650498111;0.738551062659935]
Robot Arm 1 togoal: 22.4858051513105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000085e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70846543605766;-6.1635541237109;-7.10107968783111]
Robot Arm 1 end: [-6.05864115078306;-6.77626821870602;1.02308118996825]
Robot Arm 1 togoal: 23.2286687973498
Mobile Platform 2 Pose: [-0.509408380790745;7.4649233769581;4.91650526536335]
Robot Arm 2 end: [6.95412366241636;4.26839551238477;0.58009387069631]
Robot Arm 1 togoal: 22.1791506600796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000122e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74539065977344;-6.13815410386654;-7.10000574160923]
Robot Arm 1 end: [-6.04166854763244;-6.72111221363583;0.931024287508712]
Robot Arm 1 togoal: 23.1792751136534
Mobile Platform 2 Pose: [-0.780618124191465;7.52249094162298;4.80577888361905]
Robot Arm 2 end: [7.16135451626831;4.08046544336865;0.663783433416695]
Robot Arm 1 togoal: 22.2150043028798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990307e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.7801577933704;-6.11349069526636;-7.09883348349016]
Robot Arm 1 end: [-6.02902598593042;-6.65899644314426;0.819211195116601]
Robot Arm 1 togoal: 23.1288638734587
Mobile Platform 2 Pose: [-0.593784798637588;7.34585477760238;4.62885627574335]
Robot Arm 2 end: [6.9145353433118;3.92877744027891;0.550125143889617]
Robot Arm 1 togoal: 21.932964798385

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005607e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80664854049436;-6.09323354500121;-7.09791299125623]
Robot Arm 1 end: [-5.99698278930156;-6.60117135997512;0.72961042278]
Robot Arm 1 togoal: 23.0679137840291
Mobile Platform 2 Pose: [-0.827588283014487;7.38419621321081;4.49329726899064]
Robot Arm 2 end: [7.11176346021515;3.73561161916333;0.547269268581157]
Robot Arm 1 togoal: 21.9642351746688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982733e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82845770920633;-6.07550866094781;-7.09844091879073]
Robot Arm 1 end: [-5.97863714885591;-6.56312674747323;0.646915557462607]
Robot Arm 1 togoal: 23.0308062472929
Mobile Platform 2 Pose: [-0.621457742018734;7.21617061439315;4.32261736068718]
Robot Arm 2 end: [6.83662727904711;3.61625534769457;0.43863216353721]
Robot Arm 1 togoal: 21.6805430734699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986163e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006470e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85388220094237;-6.05632412588795;-7.10075318976645]
Robot Arm 1 end: [-5.94942100554093;-6.53384031788296;0.572992436982726]
Robot Arm 1 togoal: 22.9924036239968
Mobile Platform 2 Pose: [-0.817404169080683;7.2353393936229;4.17312907609099]
Robot Arm 2 end: [7.02395318999594;3.44413974517114;0.353725067178144]
Robot Arm 1 togoal: 21.7237751431849

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985524e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.8941179432042;-6.02976880657095;-7.10427768708657]
Robot Arm 1 end: [-5.91518450945828;-6.50977210606734;0.512245147387105]
Robot Arm 1 togoal: 22.9539377736599
Mobile Platform 2 Pose: [-0.608281908251754;7.07230597210651;4.00093016556919]
Robot Arm 2 end: [6.74694372979294;3.40319051186157;0.169026800126978]
Robot Arm 1 togoal: 21.4926376166184

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983843e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006105e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.9444344933959;-5.99882299253147;-7.10842419135248]
Robot Arm 1 end: [-5.88648602959011;-6.48936343092419;0.463445232846765]
Robot Arm 1 togoal: 22.9215611774371
Mobile Platform 2 Pose: [-0.810574660018329;7.08182815417971;3.83689363546639]
Robot Arm 2 end: [6.89478947274311;3.18718969989786;0.189958664570642]
Robot Arm 1 togoal: 21.4734150648197

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981740e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982116e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99687561824313;-5.96812649261359;-7.11345880847532]
Robot Arm 1 end: [-5.86078071374533;-6.47261478774024;0.425630957439112]
Robot Arm 1 togoal: 22.8934758421301
Mobile Platform 2 Pose: [-0.649013382847012;6.93075655960215;3.64322871634227]
Robot Arm 2 end: [6.61815256004826;3.06681498307346;0.133535673915632]
Robot Arm 1 togoal: 21.1856013737413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967062e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007540e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.04769391057646;-5.94023284862455;-7.11963552537248]
Robot Arm 1 end: [-5.81117347073257;-6.46856040694549;0.389706497749304]
Robot Arm 1 togoal: 22.857963894167
Mobile Platform 2 Pose: [-0.750093239850874;6.87437295140971;3.45548958936881]
Robot Arm 2 end: [6.68657133196072;2.80415384025485;0.189316935914178]
Robot Arm 1 togoal: 21.075136966689

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.743450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.08096509243801;-5.92368443755238;-7.12435917954397]
Robot Arm 1 end: [-5.76223840520547;-6.47452683424833;0.374285480017916]
Robot Arm 1 togoal: 22.8292169849349
Mobile Platform 2 Pose: [-0.608595711406321;6.72961465064818;3.25780204718902]
Robot Arm 2 end: [6.43916177809642;2.66788332333381;0.133106090724238]
Robot Arm 1 togoal: 20.8001816665968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.829522e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007806e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.12113313223543;-5.897066272081;-7.1235663586739]
Robot Arm 1 end: [-5.76454713744639;-6.48142830132374;0.380976499566775]
Robot Arm 1 togoal: 22.8354566164547
Mobile Platform 2 Pose: [-0.685450621581498;6.66431075760383;3.06620415681721]
Robot Arm 2 end: [6.50131547425952;2.40605574435936;0.188593485909171]
Robot Arm 1 togoal: 20.6876518394906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.861974e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975884e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.15604540410606;-5.87461204883016;-7.12401281334302]
Robot Arm 1 end: [-5.80123887325553;-6.48334615351547;0.378148743640922]
Robot Arm 1 togoal: 22.862326201427
Mobile Platform 2 Pose: [-0.563339975100532;6.5332031310882;2.86532855775149]
Robot Arm 2 end: [6.30262712853392;2.24725777183368;0.132607845646501]
Robot Arm 1 togoal: 20.437676164281

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.432678e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008621e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.18291330166684;-5.85756149493503;-7.12489218320774]
Robot Arm 1 end: [-5.79888266233764;-6.49143107386474;0.368369704137816]
Robot Arm 1 togoal: 22.8670188009125
Mobile Platform 2 Pose: [-0.586674693319419;6.45714617639501;2.66830188675797]
Robot Arm 2 end: [6.29911714291194;2.00299415055256;0.187408942341171]
Robot Arm 1 togoal: 20.2857475417011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.845472e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.21049041372932;-5.83959952694803;-7.12618436239709]
Robot Arm 1 end: [-5.78891509681815;-6.50084197979334;0.357317702102567]
Robot Arm 1 togoal: 22.8674830811588
Mobile Platform 2 Pose: [-0.517123335107961;6.35197916594903;2.46453496146307]
Robot Arm 2 end: [6.20205500453105;1.81808294491484;0.131897000741119]
Robot Arm 1 togoal: 20.1023202144749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.092352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010093e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.23891556829501;-5.82089662082484;-7.12810205998733]
Robot Arm 1 end: [-5.82031904382384;-6.50760735471513;0.347992907666616]
Robot Arm 1 togoal: 22.8945308068212
Mobile Platform 2 Pose: [-0.447180792597589;6.25388077512793;2.26898601999646]
Robot Arm 2 end: [6.03676611464294;1.61337149375504;0.185740468520027]
Robot Arm 1 togoal: 19.8451803439558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.824513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.946572e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.27208082386961;-5.80005537265011;-7.12988503983552]
Robot Arm 1 end: [-5.86834361521641;-6.518500572994;0.340485801840114]
Robot Arm 1 togoal: 22.9359718355826
Mobile Platform 2 Pose: [-0.463805186687277;6.17652718663276;2.07029064586283]
Robot Arm 2 end: [6.06960925787913;1.40964939600756;0.131228253865934]
Robot Arm 1 togoal: 19.7571229319221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009477e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.30333938088459;-5.78047975336327;-7.13132191596]
Robot Arm 1 end: [-5.94405552501897;-6.54733357812253;0.337415673024279]
Robot Arm 1 togoal: 23.0093030531047
Mobile Platform 2 Pose: [-0.287822462531671;6.04288525131332;1.89290351016384]
Robot Arm 2 end: [5.826017529735;1.22708979727908;0.184272141755553]
Robot Arm 1 togoal: 19.4498642035016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.32957757907097;-5.76423310238285;-7.13251114838087]
Robot Arm 1 end: [-6.02873493845597;-6.58698438120374;0.343795062866959]
Robot Arm 1 togoal: 23.0961946227855
Mobile Platform 2 Pose: [-0.381660891090982;6.0155857652537;1.71119018859967]
Robot Arm 2 end: [5.90572983823421;1.02250264295316;0.166265289006067]
Robot Arm 1 togoal: 19.3990492637648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964403e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006462e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.35079629229192;-5.75133440564674;-7.13399490001705]
Robot Arm 1 end: [-6.12907041247754;-6.64205832033693;0.367357472421741]
Robot Arm 1 togoal: 23.2042217837529
Mobile Platform 2 Pose: [-0.166928427387217;5.86987008634178;1.55343409811868]
Robot Arm 2 end: [5.6549345335247;0.881501624710293;0.183884865549767]
Robot Arm 1 togoal: 19.1120885760896

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.931676e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.36678967482452;-5.74196382950147;-7.13699028430232]
Robot Arm 1 end: [-6.2317640942121;-6.70880055841624;0.408625875707056]
Robot Arm 1 togoal: 23.3214827552512
Mobile Platform 2 Pose: [-0.262551384595877;5.85200721177158;1.38122719042089]
Robot Arm 2 end: [5.74640040825779;0.712807097226835;0.131196019541879]
Robot Arm 1 togoal: 19.0956779618759

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000144e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.38459279775313;-5.73177955983855;-7.14335670939274]
Robot Arm 1 end: [-6.30826331382242;-6.74812021406291;0.481766586787779]
Robot Arm 1 togoal: 23.3995510449411
Mobile Platform 2 Pose: [-0.0910183692815085;5.72672961404017;1.21404912285235]
Robot Arm 2 end: [5.51110963492783;0.539827449366416;0.183851632196837]
Robot Arm 1 togoal: 18.8008098369907

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000090e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.947229e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.41001099742574;-5.71737989315176;-7.15554432428007]
Robot Arm 1 end: [-6.35968163306243;-6.801323046152;0.566686569836665]
Robot Arm 1 togoal: 23.4698605313135
Mobile Platform 2 Pose: [-0.218212041705011;5.72429565434664;1.04921657169897]
Robot Arm 2 end: [5.60848682802671;0.320676430403899;0.225587823614814]
Robot Arm 1 togoal: 18.7568471697272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000966e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.46636413043628;-5.6854247285191;-7.17927268222221]
Robot Arm 1 end: [-6.36673203972086;-6.86232380892705;0.624973981718165]
Robot Arm 1 togoal: 23.5161804832879
Mobile Platform 2 Pose: [-0.00553143299158318;5.58286284842798;0.904367594639962]
Robot Arm 2 end: [5.36921936003433;0.228956586390004;0.183787237017951]
Robot Arm 1 togoal: 18.5103375052047

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000674e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.56919300483564;-5.62672972575653;-7.21599926492117]
Robot Arm 1 end: [-6.32862143106713;-6.92373766234397;0.650759473562228]
Robot Arm 1 togoal: 23.5328301076772
Mobile Platform 2 Pose: [-0.128678371006141;5.58296546493864;0.743527594947868]
Robot Arm 2 end: [5.30287998555812;0.127272384691974;0.131467245023913]
Robot Arm 1 togoal: 18.4030047297525

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000039e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.553424e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.71653578230877;-5.54166164633621;-7.25349780807785]
Robot Arm 1 end: [-6.27313734215143;-7.0389526028679;0.598119875806516]
Robot Arm 1 togoal: 23.579495594328
Mobile Platform 2 Pose: [-0.0343776760346757;5.4369826229739;0.564468700405436]
Robot Arm 2 end: [5.05063921812717;-0.0229198498785815;0.183884649365293]
Robot Arm 1 togoal: 18.1066768529478

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996259e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.87074279237469;-5.45337967268624;-7.26167055203686]
Robot Arm 1 end: [-6.18272772070356;-7.1343174919194;0.448971268986475]
Robot Arm 1 togoal: 23.5926844169213
Mobile Platform 2 Pose: [-0.205037517259007;5.39677101072575;0.405317065296957]
Robot Arm 2 end: [5.08387525277037;-0.279171660209477;0.264281652552054]
Robot Arm 1 togoal: 17.9888058874853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989522e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.98958077880195;-5.38746646735893;-7.26135553418575]
Robot Arm 1 end: [-6.0770874263862;-7.20090849275245;0.310994659001194]
Robot Arm 1 togoal: 23.5755828351486
Mobile Platform 2 Pose: [0.0597428362379276;5.18626535485985;0.271826984417582]
Robot Arm 2 end: [4.80383918057317;-0.322791065074062;0.183584452809884]
Robot Arm 1 togoal: 17.7367050942487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986823e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-5.08220620542677;-5.33679093933145;-7.2605105043058]
Robot Arm 1 end: [-6.01442453673971;-7.24684361195829;0.275257818708787]
Robot Arm 1 togoal: 23.5683533875518
Mobile Platform 2 Pose: [0.0518368286142018;5.06624291162695;0.097286322081387]
Robot Arm 2 end: [4.74539816065344;-0.597821246576252;0.266298189701921]
Robot Arm 1 togoal: 17.5328836436081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985420e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-5.15717498678875;-5.29608387211812;-7.25957289013901]
Robot Arm 1 end: [-5.96507814163319;-7.27907649225774;0.258166555946586]
Robot Arm 1 togoal: 23.5593920295318
Mobile Platform 2 Pose: [0.195930826136089;4.9173092809492;-0.0532080648052229]
Robot Arm 2 end: [4.52343491986054;-0.651888318519962;0.196283687682338]
Robot Arm 1 togoal: 17.3225929695577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994099e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-5.20570885352463;-5.26934265357912;-7.25855704007182]
Robot Arm 1 end: [-5.92073613992932;-7.21556086076421;0.240966675532015]
Robot Arm 1 togoal: 23.4836934978226
Mobile Platform 2 Pose: [0.215815669041879;4.79578599999773;-0.220948267734166]
Robot Arm 2 end: [4.42049063540366;-0.895693014826852;0.279584150874508]
Robot Arm 1 togoal: 17.0991248067408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-5.25663847479109;-5.24156544506139;-7.25732151466711]
Robot Arm 1 end: [-5.86359001992895;-7.16731529922519;0.224292397686971]
Robot Arm 1 togoal: 23.4105030597229
Mobile Platform 2 Pose: [0.257008997391349;4.68920568132964;-0.372793934458711]
Robot Arm 2 end: [4.27684839497528;-0.972771268809025;0.216873060726031]
Robot Arm 1 togoal: 16.9416647863216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-5.30145387317734;-5.21673363837041;-7.25644369708614]
Robot Arm 1 end: [-5.77829345315802;-7.05757528681818;0.202758179041255]
Robot Arm 1 togoal: 23.2734436742388
Mobile Platform 2 Pose: [0.299994638069135;4.54892261469276;-0.539841741844693]
Robot Arm 2 end: [4.11321475906265;-1.1881837286489;0.301592991938202]
Robot Arm 1 togoal: 16.6828621731039

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.022421e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-5.36128385212547;-5.18489491209981;-7.25444708137085]
Robot Arm 1 end: [-5.67934852955957;-6.94630563937893;0.192885375863185]
Robot Arm 1 togoal: 23.1253785609986
Mobile Platform 2 Pose: [0.271841730477994;4.46093646458918;-0.687219806102062]
Robot Arm 2 end: [4.09105604154263;-1.3306607277214;0.244080347253982]
Robot Arm 1 togoal: 16.5935477696403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.061782e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-5.45517122211748;-5.13831562083919;-7.2482366432106]
Robot Arm 1 end: [-5.5897302545969;-6.8498020610335;0.201152666785915]
Robot Arm 1 togoal: 22.9934233182033
Mobile Platform 2 Pose: [0.376794571938328;4.2994436691855;-0.855626467163346]
Robot Arm 2 end: [3.80112946902638;-1.458463044529;0.30791604557055]
Robot Arm 1 togoal: 16.275767289532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.197384e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-5.58045539487829;-5.07743608934358;-7.23736186953361]
Robot Arm 1 end: [-5.5531261259136;-6.76357581955586;0.296292164965901]
Robot Arm 1 togoal: 22.9001553668915
Mobile Platform 2 Pose: [0.222399941515321;4.27009665320383;-0.977216562067069]
Robot Arm 2 end: [3.91950604914449;-1.63011924698194;0.251710891281704]
Robot Arm 1 togoal: 16.2916741715952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.440934e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-5.69995458530173;-5.02553166575154;-7.22105924155321]
Robot Arm 1 end: [-5.50451176127079;-6.67928943918202;0.403449133307378]
Robot Arm 1 togoal: 22.799945199299
Mobile Platform 2 Pose: [0.127503547481597;4.19897945199842;-1.15221278636751]
Robot Arm 2 end: [3.93949142403672;-1.87090695952439;0.302598340861143]
Robot Arm 1 togoal: 16.182585622344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.171348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-5.82705560226274;-4.98554211717979;-7.19021697160413]
Robot Arm 1 end: [-5.37896331743658;-6.55153480091361;0.502336425514309]
Robot Arm 1 togoal: 22.6164429666316
Mobile Platform 2 Pose: [0.305685527723009;4.03132125250903;-1.30681258483561]
Robot Arm 2 end: [3.6768164035756;-1.93790503764009;0.228581655817269]
Robot Arm 1 togoal: 15.9286856738277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.652211e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-6.04621096824961;-4.9185256769028;-7.12102627439996]
Robot Arm 1 end: [-5.31911810551098;-6.46874834606693;0.618539445714008]
Robot Arm 1 togoal: 22.5102084990059
Mobile Platform 2 Pose: [0.174748745402779;3.97901383765293;-1.47450066559451]
Robot Arm 2 end: [3.70642692179218;-2.19471782261586;0.312324727189677]
Robot Arm 1 togoal: 15.8192751017903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.316257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-6.09535534666482;-4.93180078411669;-7.07356304741707]
Robot Arm 1 end: [-5.27783052969691;-6.4014969228197;0.540231200800513]
Robot Arm 1 togoal: 22.4362009805779
Mobile Platform 2 Pose: [0.410150626558826;3.7900226628962;-1.61304365676075]
Robot Arm 2 end: [3.38145259435748;-2.21964506347226;0.228309132459726]
Robot Arm 1 togoal: 15.5328119203321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.139766e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-6.09129957006293;-4.96753462600206;-7.03252361473416]
Robot Arm 1 end: [-5.27333516839169;-6.35758451518162;0.459168814546375]
Robot Arm 1 togoal: 22.4047500277004
Mobile Platform 2 Pose: [0.281859739935686;3.73586299438403;-1.76765092680775]
Robot Arm 2 end: [3.40232424116124;-2.47049926235485;0.313149959781465]
Robot Arm 1 togoal: 15.419940925408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.645179e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-6.08109460028138;-4.99250131905402;-7.00759068550773]
Robot Arm 1 end: [-5.22364655706998;-6.3191385615756;0.406399315918733]
Robot Arm 1 togoal: 22.3453805644801
Mobile Platform 2 Pose: [0.489652331372771;3.5524795981142;-1.90510548164006]
Robot Arm 2 end: [3.10761373165315;-2.4861053142139;0.228380544662938]
Robot Arm 1 togoal: 15.1637401766063

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970892e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917669e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-6.07503164776574;-5.00541957596719;-6.99437738524904]
Robot Arm 1 end: [-5.14659749998933;-6.30435251917069;0.35348446291717]
Robot Arm 1 togoal: 22.2848473054091
Mobile Platform 2 Pose: [0.443776464412983;3.44902242750987;-2.06295967986259]
Robot Arm 2 end: [3.07516667636555;-2.74789947880795;0.307318463552797]
Robot Arm 1 togoal: 15.0008655902293

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003907e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-6.08978661082735;-5.00160451122986;-6.98815252608542]
Robot Arm 1 end: [-5.07811501874462;-6.28627278716135;0.353546318189064]
Robot Arm 1 togoal: 22.2251025175475
Mobile Platform 2 Pose: [0.573570992075174;3.29898188679128;-2.20647349883052]
Robot Arm 2 end: [2.85228185042481;-2.80064411916966;0.251193713297519]
Robot Arm 1 togoal: 14.7859487983475

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980491e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.552926e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-6.13267968069514;-4.98020952527601;-6.9845739400501]
Robot Arm 1 end: [-5.0160902736884;-6.27878331821139;0.364302152887228]
Robot Arm 1 togoal: 22.1770084397863
Mobile Platform 2 Pose: [0.597797629574186;3.16877617075687;-2.36144285204619]
Robot Arm 2 end: [2.73771759064382;-3.02996402344821;0.338696006193381]
Robot Arm 1 togoal: 14.5680710345328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004259e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-6.20918730937398;-4.94009096347658;-6.98039798941912]
Robot Arm 1 end: [-4.92417492018283;-6.26338570871349;0.395881717807906]
Robot Arm 1 togoal: 22.1019140100473
Mobile Platform 2 Pose: [0.603552710432703;3.07367013862373;-2.50140291587074]
Robot Arm 2 end: [2.65387588704811;-3.15973036115974;0.323754812167514]
Robot Arm 1 togoal: 14.4341005863422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989707e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928843e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-6.30969399461803;-4.88771934282777;-6.97386845340508]
Robot Arm 1 end: [-4.7438246482113;-6.27714423090509;0.380822996603706]
Robot Arm 1 togoal: 21.9914936548194
Mobile Platform 2 Pose: [0.679754211002228;2.93307048799192;-2.65173282096977]
Robot Arm 2 end: [2.43620958183199;-3.30349219874445;0.414313919916869]
Robot Arm 1 togoal: 14.1678255062439

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003009e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-6.40063805373437;-4.84052946724276;-6.96681558114935]
Robot Arm 1 end: [-4.5824962025133;-6.31850898003126;0.348940118357904]
Robot Arm 1 togoal: 21.9161277417757
Mobile Platform 2 Pose: [0.581872760137311;2.8846685694685;-2.77607052294124]
Robot Arm 2 end: [2.53105332298776;-3.51453385895636;0.444676203737006]
Robot Arm 1 togoal: 14.1508671594566

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004244e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-6.49724659500862;-4.79070094346509;-6.95672625757915]
Robot Arm 1 end: [-4.43644052928664;-6.36319376212626;0.34366205025641]
Robot Arm 1 togoal: 21.8529414473298
Mobile Platform 2 Pose: [0.716844482292956;2.73770812008602;-2.91880707470204]
Robot Arm 2 end: [2.22531003308189;-3.58477585197613;0.53423742147388]
Robot Arm 1 togoal: 13.8414958245394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.018325e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-6.60905043078207;-4.73398643367276;-6.94068192769001]
Robot Arm 1 end: [-4.30979042898111;-6.40695271109479;0.357495667949052]
Robot Arm 1 togoal: 21.8016499769206
Mobile Platform 2 Pose: [0.548888596401817;2.73622169394926;-3.02674147738902]
Robot Arm 2 end: [2.37429794905739;-3.76597409474728;0.50722534559781]
Robot Arm 1 togoal: 13.8929502903205

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001569e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.751928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-6.66751471820831;-4.70494649230134;-6.92798201358253]
Robot Arm 1 end: [-4.22692073023222;-6.48365466071261;0.342228064361428]
Robot Arm 1 togoal: 21.806091986282
Mobile Platform 2 Pose: [0.609694758321924;2.61632932830416;-3.18468821672225]
Robot Arm 2 end: [2.3233186664326;-3.95575653420355;0.484707469844575]
Robot Arm 1 togoal: 13.7648452311069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.772106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-6.67318143815337;-4.70337866844037;-6.91962184880067]
Robot Arm 1 end: [-4.20941372299515;-6.5996201477753;0.278447847579459]
Robot Arm 1 togoal: 21.8860403740182
Mobile Platform 2 Pose: [0.696631271782958;2.51231059606496;-3.32664152369698]
Robot Arm 2 end: [2.18238084952868;-4.04649010006387;0.392247556896032]
Robot Arm 1 togoal: 13.6062031944777

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.633952e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.559324e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-6.66403302421123;-4.70900911658569;-6.91394596629637]
Robot Arm 1 end: [-4.20212198916929;-6.73305201094483;0.224667017428678]
Robot Arm 1 togoal: 21.9858085545638
Mobile Platform 2 Pose: [0.7749695854305;2.39966001102084;-3.47674020588671]
Robot Arm 2 end: [2.14367260278907;-4.1801082900211;0.26949494987329]
Robot Arm 1 togoal: 13.5242814481991

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.659029e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.519394e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-6.6607984440957;-4.71009100231004;-6.91108548262538]
Robot Arm 1 end: [-4.18226925469954;-6.85953182319914;0.18838419946348]
Robot Arm 1 togoal: 22.0716206786475
Mobile Platform 2 Pose: [0.946837585274704;2.26959343961136;-3.60364474188276]
Robot Arm 2 end: [1.93662847452542;-4.21441713123718;0.190899747246188]
Robot Arm 1 togoal: 13.3313628392805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.406673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.499575e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-6.68307643528994;-4.69490096325393;-6.90994958398631]
Robot Arm 1 end: [-4.14160900576381;-6.88829803947317;0.18021853035969]
Robot Arm 1 togoal: 22.0680178287789
Mobile Platform 2 Pose: [1.0927846577915;2.12867553963542;-3.74750550756235]
Robot Arm 2 end: [1.72111336419654;-4.27344482828658;0.13227441885794]
Robot Arm 1 togoal: 13.1189363658318

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.090173e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.666320e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-6.73756894219183;-4.65901078460563;-6.90906490703908]
Robot Arm 1 end: [-4.11457005161359;-7.01868725774113;0.168274860081415]
Robot Arm 1 togoal: 22.1514484633144
Mobile Platform 2 Pose: [1.23501479792731;1.97580845560048;-3.88311570090752]
Robot Arm 2 end: [1.46846762101915;-4.3577584574347;0.184240110603421]
Robot Arm 1 togoal: 12.8509753015448

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.224716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-6.85547148224186;-4.57572170720727;-6.90471585183628]
Robot Arm 1 end: [-4.03264532845043;-6.99491796604852;0.152458399633962]
Robot Arm 1 togoal: 22.0820125373285
Mobile Platform 2 Pose: [1.10296038747061;1.92978963664249;-3.99949777121807]
Robot Arm 2 end: [1.50078696655509;-4.54458055862681;0.159310680163294]
Robot Arm 1 togoal: 12.801714726362

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.169111e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-6.93544079340971;-4.51323278028765;-6.90294137323569]
Robot Arm 1 end: [-3.96131787412204;-7.11793370211924;0.189911602109805]
Robot Arm 1 togoal: 22.1294791474049
Mobile Platform 2 Pose: [1.20005594436101;1.76741026377561;-4.14603417987106]
Robot Arm 2 end: [1.2552178191135;-4.62405113191176;0.183479486608243]
Robot Arm 1 togoal: 12.5447086658771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.714450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-7.08891297092233;-4.36493199861775;-6.88923696431554]
Robot Arm 1 end: [-3.85003231249959;-7.23921542425731;0.230904909779093]
Robot Arm 1 togoal: 22.1512593060315
Mobile Platform 2 Pose: [1.13323766378529;1.64520297060623;-4.2885000842164]
Robot Arm 2 end: [1.18411233645885;-4.87396184488964;0.23262043665734]
Robot Arm 1 togoal: 12.3701478496183

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.611637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-7.18924146518659;-4.14985202884496;-6.85534515077214]
Robot Arm 1 end: [-3.71321811854725;-7.32151277469507;0.269436179862773]
Robot Arm 1 togoal: 22.1281171355959
Mobile Platform 2 Pose: [1.22617173758849;1.48274354262229;-4.42434480357062]
Robot Arm 2 end: [0.969018088359349;-4.90635477571824;0.183347946176011]
Robot Arm 1 togoal: 12.16773982754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.680020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-7.0123873755162;-4.07356309843271;-6.80480668814556]
Robot Arm 1 end: [-3.52398594347161;-7.21643372100647;0.331581417724656]
Robot Arm 1 togoal: 21.9252936681105
Mobile Platform 2 Pose: [1.26045338225971;1.30142478531979;-4.57135766835812]
Robot Arm 2 end: [0.76884920922889;-5.11726328805665;0.25914980488781]
Robot Arm 1 togoal: 11.8912361428143
Step 113:
Mobile Platform 1 Pose: [-6.89892210600554;-3.95347355677455;-6.76503501643298]
Robot Arm 1 end: [-3.29741949810113;-7.0959110547247;0.427433971667545]
Robot Arm 1 togoal: 21.6861113793566
Mobile Platform 2 Pose: [1.18943169507099;1.21491963484798;-4.68786344843959]
Robot Arm 2 end: [0.793447166690839;-5.29101727153305;0.229898420734383]
Robot Arm 1 togoal: 11.8465169296772
Step 114:
Mobile Platform 1 Pose: [-6.936638391241;-3.77624047815043;-6.73546790121488]
Robot Arm 1 end: [-3.1772536272163;-7.18340095866335;0.500063843771867]
Robot Arm 1 togoal: 21.6783669195184
Mobile Platform 2 Pose: [1.31290331746463;1.02826105722398;-4.83697933691065]
Robot Arm 2 end: [0.460123807916307;-5.36739651176189;0.288449527467718]
Robot Arm 1 togoal: 11.5062759320083
Step 115:
Mobile Platform 1 Pose: [-6.85466382793675;-3.61362321832254;-6.69324659103733]
Robot Arm 1 end: [-3.03061868087801;-7.1881408184433;0.619663218304817]
Robot Arm 1 togoal: 21.5879970352768
Mobile Platform 2 Pose: [1.13858916345656;1.00288122383972;-4.93071138491906]
Robot Arm 2 end: [0.552489304621556;-5.57072152758422;0.331336610343064]
Robot Arm 1 togoal: 11.5060348205213
Step 116:
Mobile Platform 1 Pose: [-6.94778797901333;-3.39733235999346;-6.67854748188603]
Robot Arm 1 end: [-2.90617839262312;-7.27468389862933;0.625743828379133]
Robot Arm 1 togoal: 21.5820672807913
Mobile Platform 2 Pose: [1.0708427639485;0.910550457644643;-5.07398727379761]
Robot Arm 2 end: [0.543404879588868;-5.80165039281525;0.423155858218846]
Robot Arm 1 togoal: 11.4015256715376
Step 117:
Mobile Platform 1 Pose: [-7.20384701916781;-3.18445122904601;-6.691585146198]
Robot Arm 1 end: [-2.68147685686098;-7.29386679881052;0.502518524789184]
Robot Arm 1 togoal: 21.4693965597883
Mobile Platform 2 Pose: [1.2359705995156;0.748359611671163;-5.19669288207919]
Robot Arm 2 end: [0.299938490434819;-5.84800008867433;0.359707074356415]
Robot Arm 1 togoal: 11.1658603533064
Step 118:
Mobile Platform 1 Pose: [-7.41404060592516;-3.03621663554576;-6.70166629095911]
Robot Arm 1 end: [-2.45703963630407;-7.17651255071513;0.315986371593135]
Robot Arm 1 togoal: 21.2523522874521
Mobile Platform 2 Pose: [1.22722676897242;0.64519788264542;-5.33596626278876]
Robot Arm 2 end: [0.262939559562807;-6.06315539079023;0.451495867431729]
Robot Arm 1 togoal: 11.0440261213872
Step 119:
Mobile Platform 1 Pose: [-7.64963983789657;-2.80262910256977;-6.70028421162645]
Robot Arm 1 end: [-2.16586377017471;-7.07135966094079;0.235360688262492]
Robot Arm 1 togoal: 21.0022006082108
Mobile Platform 2 Pose: [1.33604838433551;0.51602500209448;-5.46046425436278]
Robot Arm 2 end: [0.0401204986675609;-6.11392403126354;0.439289487016981]
Robot Arm 1 togoal: 10.8201526651177
Step 120:
Mobile Platform 1 Pose: [-7.85587772913171;-2.63677284419812;-6.69790984382522]
Robot Arm 1 end: [-1.97995032491006;-6.9258557093434;0.142711567615657]
Robot Arm 1 togoal: 20.7822876576105
Mobile Platform 2 Pose: [1.3461610216987;0.404612242207312;-5.59646489778898]
Robot Arm 2 end: [-0.0349591416413891;-6.30019115193933;0.531886324247955]
Robot Arm 1 togoal: 10.6756250723086
Step 121:
Mobile Platform 1 Pose: [-7.9922839933853;-2.44646537538022;-6.67377838119773]
Robot Arm 1 end: [-1.71590298998635;-6.80159150418907;0.202926222618266]
Robot Arm 1 togoal: 20.5254276064357
Mobile Platform 2 Pose: [1.39658711907579;0.292344608684623;-5.72488985001931]
Robot Arm 2 end: [-0.214038138980478;-6.36499446343903;0.523227647822095]
Robot Arm 1 togoal: 10.4868424448415
Step 122:
Mobile Platform 1 Pose: [-8.04591316314624;-2.30499526786296;-6.64193034308075]
Robot Arm 1 end: [-1.52608831413759;-6.72347395319021;0.269813885662158]
Robot Arm 1 togoal: 20.3492201221826
Mobile Platform 2 Pose: [1.44688885996083;0.149324110034566;-5.86330165520615]
Robot Arm 2 end: [-0.397698259745338;-6.48650894143838;0.616476471350027]
Robot Arm 1 togoal: 10.264841983361
Step 123:
Mobile Platform 1 Pose: [-8.10380892007258;-2.17285899211927;-6.6103530679969]
Robot Arm 1 end: [-1.38536908306325;-6.59573262320015;0.281908136360443]
Robot Arm 1 togoal: 20.1638369142597
Mobile Platform 2 Pose: [1.35345205854164;0.0851997113802557;-5.9847019686628]
Robot Arm 2 end: [-0.312811299759911;-6.69514558343028;0.616905486176631]
Robot Arm 1 togoal: 10.2752651799701
Step 124:
Mobile Platform 1 Pose: [-8.00757284194296;-2.135923685773;-6.57551257736843]
Robot Arm 1 end: [-1.33562965837225;-6.68502984966041;0.360823460528599]
Robot Arm 1 togoal: 20.2047699922213
Mobile Platform 2 Pose: [1.5787965104421;-0.0995836227694824;-6.09525992141659]
Robot Arm 2 end: [-0.641447800953169;-6.71329689896166;0.642127521357489]
Robot Arm 1 togoal: 9.95777753167716
Step 125:
Mobile Platform 1 Pose: [-8.07740117483789;-1.97680966054558;-6.53757657088091]
Robot Arm 1 end: [-1.18844430934295;-6.51793742287997;0.307279538528777]
Robot Arm 1 togoal: 19.9874125420986
Mobile Platform 2 Pose: [1.48919449220685;-0.166721283216628;-6.21876572362674]
Robot Arm 2 end: [-0.559565700674026;-6.92443399792768;0.619808277925815]
Robot Arm 1 togoal: 9.96960643721348
Step 126:
Mobile Platform 1 Pose: [-7.95143847083395;-1.93755050080323;-6.49161048700814]
Robot Arm 1 end: [-1.14277186749975;-6.59590769784263;0.38702073961878]
Robot Arm 1 togoal: 20.0217738017747
Mobile Platform 2 Pose: [1.71723443987706;-0.34944008810891;-6.32458703355715]
Robot Arm 2 end: [-0.888190986786138;-6.93824493754424;0.621867167049941]
Robot Arm 1 togoal: 9.65442113267531
Step 127:
Mobile Platform 1 Pose: [-8.07236786618142;-1.73666803146243;-6.44347678757348]
Robot Arm 1 end: [-0.954952455936468;-6.42029894822375;0.305876506451863]
Robot Arm 1 togoal: 19.7765954088523
Mobile Platform 2 Pose: [1.63155071987068;-0.417324493069517;-6.44689566828161]
Robot Arm 2 end: [-0.80640682003068;-7.14867173395352;0.576226335142348]
Robot Arm 1 togoal: 9.67185740782723
Step 128:
Mobile Platform 1 Pose: [-7.93019882921765;-1.69693419454254;-6.38936973153131]
Robot Arm 1 end: [-0.910803600266968;-6.4896193835514;0.385763933723253]
Robot Arm 1 togoal: 19.8050989279066
Mobile Platform 2 Pose: [1.85646555847411;-0.597341795611585;-6.55112764106269]
Robot Arm 2 end: [-1.13396638694328;-7.15940422540363;0.542494914552844]
Robot Arm 1 togoal: 9.36125037507021

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999042e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-7.9977054752152;-1.51384289190696;-6.33299277250824]
Robot Arm 1 end: [-0.748896865154974;-6.30666633251508;0.27901540024035]
Robot Arm 1 togoal: 19.5700964852951
Mobile Platform 2 Pose: [1.75774002449124;-0.655033297411232;-6.66832093762391]
Robot Arm 2 end: [-1.03950604219206;-7.36072324553559;0.482924606239657]
Robot Arm 1 togoal: 9.39860810320866

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001006e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-7.82546718834726;-1.48525889196538;-6.27119311153176]
Robot Arm 1 end: [-0.719786096250915;-6.37259963660229;0.35561349441004]
Robot Arm 1 togoal: 19.6044372150096
Mobile Platform 2 Pose: [1.97179732695164;-0.835735142785885;-6.78114277870358]
Robot Arm 2 end: [-1.36175710324531;-7.3709801718673;0.405635202736075]
Robot Arm 1 togoal: 9.09807805149106
Step 131:
Mobile Platform 1 Pose: [-8.07069750707972;-1.24585285335844;-6.22669487505884]
Robot Arm 1 end: [-0.479002231888994;-6.20589851815393;0.277340755585449]
Robot Arm 1 togoal: 19.3387518437167
Mobile Platform 2 Pose: [1.81928895736517;-0.856987024964209;-6.88407368470141]
Robot Arm 2 end: [-1.2325667270509;-7.55410384403047;0.368939175879296]
Robot Arm 1 togoal: 9.17483178831499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000011e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-8.09221682985329;-1.08931106854915;-6.15480776949113]
Robot Arm 1 end: [-0.27948195452533;-6.1989306137822;0.363437152710873]
Robot Arm 1 togoal: 19.2201210721724
Mobile Platform 2 Pose: [1.96709265266274;-1.02556728579307;-7.02298529131131]
Robot Arm 2 end: [-1.53041621725767;-7.58039720130786;0.286613794290638]
Robot Arm 1 togoal: 8.89449472770524
Step 133:
Mobile Platform 1 Pose: [-8.26148460167873;-0.924768461772719;-6.12560674973415]
Robot Arm 1 end: [-0.14051790156841;-6.03888057907055;0.282831608145007]
Robot Arm 1 togoal: 19.0160157369599
Mobile Platform 2 Pose: [1.78872432595321;-1.05114948591465;-7.13256281762057]
Robot Arm 2 end: [-1.40409120392536;-7.75430687939758;0.241468384823079]
Robot Arm 1 togoal: 8.97607858569607
Step 134:
Mobile Platform 1 Pose: [-8.17533907370169;-0.873076008775001;-6.07802903658305]
Robot Arm 1 end: [-0.0535588027003574;-6.1234859531222;0.401494800455657]
Robot Arm 1 togoal: 19.0340257373917
Mobile Platform 2 Pose: [1.87760527921083;-1.22323507979289;-7.2957664986924]
Robot Arm 2 end: [-1.58803049035855;-7.87208037815574;0.159094675739034]
Robot Arm 1 togoal: 8.78316907843733
Step 135:
Mobile Platform 1 Pose: [-8.30997888888269;-0.697650150960145;-6.03835039938105]
Robot Arm 1 end: [0.0962156715643934;-5.94898518302609;0.313441968949928]
Robot Arm 1 togoal: 18.8125827251578
Mobile Platform 2 Pose: [1.79901327502318;-1.32319670822896;-7.44974688949296]
Robot Arm 2 end: [-1.61541241728734;-8.0737850795448;0.186058953131065]
Robot Arm 1 togoal: 8.70595660743718
Step 136:
Mobile Platform 1 Pose: [-8.2094930999755;-0.65285201791632;-5.9911541612905]
Robot Arm 1 end: [0.173350527143255;-6.03816934922512;0.448917200056051]
Robot Arm 1 togoal: 18.8397260454274
Mobile Platform 2 Pose: [1.79194518414262;-1.4714232825654;-7.62474815948492]
Robot Arm 2 end: [-1.67378359647562;-8.29698906817279;0.177761838006301]
Robot Arm 1 togoal: 8.6040928976899
Step 137:
Mobile Platform 1 Pose: [-8.36576235635492;-0.468208229176652;-5.95280677676046]
Robot Arm 1 end: [0.322415555844587;-5.8593025699833;0.31112464151518]
Robot Arm 1 togoal: 18.6181878028745
Mobile Platform 2 Pose: [2.00457362112891;-1.6550574805336;-7.74901234071797]
Robot Arm 2 end: [-1.95156221317266;-8.400961880391;0.184182062910762]
Robot Arm 1 togoal: 8.31392542144301
Step 138:
Mobile Platform 1 Pose: [-8.25236232018095;-0.434451725190657;-5.90740311021215]
Robot Arm 1 end: [0.401368540036744;-5.96650366099836;0.511359661569607]
Robot Arm 1 togoal: 18.6569648837601
Mobile Platform 2 Pose: [2.01359578790808;-1.7568123137756;-7.87574892492364]
Robot Arm 2 end: [-2.02244054023128;-8.58497738014948;0.246467127774401]
Robot Arm 1 togoal: 8.20171799022727
Step 139:
Mobile Platform 1 Pose: [-8.42135258603238;-0.273827870706161;-5.88203413343223]
Robot Arm 1 end: [0.555232114962417;-5.78520220269573;0.429157354634897]
Robot Arm 1 togoal: 18.4273809681264
Mobile Platform 2 Pose: [2.15406909280568;-1.87430577335461;-7.95575179612074]
Robot Arm 2 end: [-2.23153878078918;-8.62136274720376;0.248710662316721]
Robot Arm 1 togoal: 7.99176767354774
Step 140:
Mobile Platform 1 Pose: [-8.3335513999348;-0.224225650159727;-5.83797295643761]
Robot Arm 1 end: [0.620862595511476;-5.89957125657631;0.613983960962441]
Robot Arm 1 togoal: 18.4820795012438
Mobile Platform 2 Pose: [2.22741953941645;-1.98463917297751;-8.05790324666672]
Robot Arm 2 end: [-2.41407798092483;-8.71541125386913;0.31152101588332]
Robot Arm 1 togoal: 7.78840295177359
Step 141:
Mobile Platform 1 Pose: [-8.49359777855191;-0.0751058337831857;-5.81724516773102]
Robot Arm 1 end: [0.775123880096019;-5.71960031713387;0.546477700345159]
Robot Arm 1 togoal: 18.2525030412416
Mobile Platform 2 Pose: [2.2750777235225;-2.08157671616771;-8.14828305565166]
Robot Arm 2 end: [-2.58082803414812;-8.75792930075609;0.343423053496677]
Robot Arm 1 togoal: 7.61403570688625
Step 142:
Mobile Platform 1 Pose: [-8.39629515611242;-0.0458010892286483;-5.78328657480838]
Robot Arm 1 end: [0.791059594289728;-5.84330239656014;0.760717690154247]
Robot Arm 1 togoal: 18.3410153321847
Mobile Platform 2 Pose: [2.28591642405577;-2.19587139047941;-8.25822940134923]
Robot Arm 2 end: [-2.70477974536615;-8.89844279628034;0.433797603594395]
Robot Arm 1 togoal: 7.45759182896441
Step 143:
Mobile Platform 1 Pose: [-8.54970347199656;0.0986742344488646;-5.76482875729924]
Robot Arm 1 end: [0.952068714781284;-5.66713690593934;0.69350680149619]
Robot Arm 1 togoal: 18.1110183138623
Mobile Platform 2 Pose: [2.30965417418908;-2.2975743385938;-8.35435371412853]
Robot Arm 2 end: [-2.85782258554459;-8.94413924494065;0.470961226939397]
Robot Arm 1 togoal: 7.29576052091638
Step 144:
Mobile Platform 1 Pose: [-8.45690833146357;0.127542964149746;-5.73449165361964]
Robot Arm 1 end: [0.930660983251558;-5.79239833771227;0.904882927266534]
Robot Arm 1 togoal: 18.2217550116614
Mobile Platform 2 Pose: [2.33468049529225;-2.43367088375566;-8.46763282390664]
Robot Arm 2 end: [-3.03636441539888;-9.03828672597429;0.563413351339698]
Robot Arm 1 togoal: 7.09465187068772
Step 145:
Mobile Platform 1 Pose: [-8.60789123875884;0.266225257534395;-5.71978970265752]
Robot Arm 1 end: [1.09665730249617;-5.62495432278713;0.845128457666059]
Robot Arm 1 togoal: 17.9962638100278
Mobile Platform 2 Pose: [2.24853584437621;-2.50324368618488;-8.56432842527091]
Robot Arm 2 end: [-3.00166982101736;-9.21146135965068;0.617696364732606]
Robot Arm 1 togoal: 7.10030815802177
Step 146:
Mobile Platform 1 Pose: [-8.51677028396257;0.295375473417966;-5.69306668324399]
Robot Arm 1 end: [1.03920880341035;-5.74742504442529;1.04285618059181]
Robot Arm 1 togoal: 18.124728850681
Mobile Platform 2 Pose: [2.42446535284212;-2.68002057910679;-8.66210796311851]
Robot Arm 2 end: [-3.30480961709462;-9.19569957652444;0.6617385294407]
Robot Arm 1 togoal: 6.79785287494068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-8.66760714793213;0.429057988372364;-5.68336887594371]
Robot Arm 1 end: [1.21504025120383;-5.59319593843594;0.978806048665731]
Robot Arm 1 togoal: 17.9057882161967
Mobile Platform 2 Pose: [2.29850382249372;-2.73337731922909;-8.75613090925106]
Robot Arm 2 end: [-3.22161823259036;-9.39272571915878;0.70282049250635]
Robot Arm 1 togoal: 6.85453566212951
Step 148:
Mobile Platform 1 Pose: [-8.62210023122393;0.507267796918907;-5.64876983718499]
Robot Arm 1 end: [1.18546171303594;-5.61625153677324;1.14751401875266]
Robot Arm 1 togoal: 17.9360778614032
Mobile Platform 2 Pose: [2.48692047829881;-2.90883589405009;-8.84641302563401]
Robot Arm 2 end: [-3.52101412051416;-9.36971634443912;0.729812239777377]
Robot Arm 1 togoal: 6.55747615969514
Step 149:
Mobile Platform 1 Pose: [-8.79782658936643;0.65492820377537;-5.6360441379101]
Robot Arm 1 end: [1.384978672607;-5.45883647059921;1.05989590419968]
Robot Arm 1 togoal: 17.7033001014875
Mobile Platform 2 Pose: [2.37997118022795;-2.97522889065766;-8.94442745759473]
Robot Arm 2 end: [-3.45377108059879;-9.57284490152974;0.729008196917879]
Robot Arm 1 togoal: 6.60778522346959
Step 150:
Mobile Platform 1 Pose: [-8.76880247596516;0.769992350356776;-5.58242175293459]
Robot Arm 1 end: [1.39458185456064;-5.38256430132157;1.17714044462234]
Robot Arm 1 togoal: 17.6293716659436
Mobile Platform 2 Pose: [2.55702504565056;-3.13837977596102;-9.02646483748974]
Robot Arm 2 end: [-3.74402625050228;-9.54392520289321;0.729324423387445]
Robot Arm 1 togoal: 6.3223383327346
Step 151:
Mobile Platform 1 Pose: [-8.99171204035461;0.93993380797977;-5.56334147993352]
Robot Arm 1 end: [1.62645529928521;-5.19087807660356;1.00145272179129]
Robot Arm 1 togoal: 17.3536439103549
Mobile Platform 2 Pose: [2.47316376367946;-3.21010867606004;-9.11885931777872]
Robot Arm 2 end: [-3.70642086355679;-9.73614376822722;0.676859457572634]
Robot Arm 1 togoal: 6.35541750895157
Step 152:
Mobile Platform 1 Pose: [-8.91888873752951;1.0616390672514;-5.47363257788783]
Robot Arm 1 end: [1.60471046919867;-5.07294680411034;1.15293203267225]
Robot Arm 1 togoal: 17.2571749047779
Mobile Platform 2 Pose: [2.6201301111022;-3.36116227419152;-9.20001103336816]
Robot Arm 2 end: [-3.986676587479;-9.70311911521689;0.643334776767536]
Robot Arm 1 togoal: 6.0842824529598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.168391e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-9.13282766622693;1.2070136151233;-5.45640169854447]
Robot Arm 1 end: [1.83903559771543;-4.98158060489928;0.98040467671202]
Robot Arm 1 togoal: 17.0687240501454
Mobile Platform 2 Pose: [2.52052965661113;-3.41520212819432;-9.27984070587993]
Robot Arm 2 end: [-3.91443410454152;-9.90014969083502;0.55759713379981]
Robot Arm 1 togoal: 6.16216093873006

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.667433e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-9.36401367098454;1.43482659278182;-5.38781714298947]
Robot Arm 1 end: [2.15421492775861;-4.98400746335425;0.924298334753344]
Robot Arm 1 togoal: 16.9243279316778
Mobile Platform 2 Pose: [2.66983405983197;-3.56745638583113;-9.36192909580786]
Robot Arm 2 end: [-4.1919634709183;-9.86202831832246;0.492225327091502]
Robot Arm 1 togoal: 5.90005947730278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.673919e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-9.35939527952002;1.58972664691754;-5.30285765974153]
Robot Arm 1 end: [2.29616303259963;-4.9565796853473;1.03177067162845]
Robot Arm 1 togoal: 16.8311534138385
Mobile Platform 2 Pose: [2.55374234008545;-3.61306481186088;-9.44064472419642]
Robot Arm 2 end: [-4.10075960037637;-10.0523954752453;0.414730993080669]
Robot Arm 1 togoal: 6.00230070006893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-9.59082801066443;1.75161645365292;-5.296193287183]
Robot Arm 1 end: [2.53253300802259;-5.04887563056725;0.93927071965877]
Robot Arm 1 togoal: 16.8098224254797
Mobile Platform 2 Pose: [2.71252246029959;-3.77565494693104;-9.5292014114631]
Robot Arm 2 end: [-4.38899637065753;-10.0252202819545;0.376840148372891]
Robot Arm 1 togoal: 5.7265259588333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-9.59516244632176;1.76069168485639;-5.29716072723784]
Robot Arm 1 end: [2.53783046319156;-5.14864150078605;1.00166748685463]
Robot Arm 1 togoal: 16.8948222770948
Mobile Platform 2 Pose: [2.58378306601929;-3.80015190018456;-9.59544492672753]
Robot Arm 2 end: [-4.2928180735387;-10.2104847482724;0.383289887061881]
Robot Arm 1 togoal: 5.82328203610846

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.335594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-9.63795505279914;1.79225645856821;-5.29740118440881]
Robot Arm 1 end: [2.50429821220884;-5.12064791104915;1.09835237981056]
Robot Arm 1 togoal: 16.8818888028639
Mobile Platform 2 Pose: [2.69622465278314;-3.94530581972399;-9.68566297728218]
Robot Arm 2 end: [-4.55764011820556;-10.2149038603893;0.413308354508759]
Robot Arm 1 togoal: 5.55809729436854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999439e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-9.7224258029876;1.84591365828772;-5.29534436565697]
Robot Arm 1 end: [2.4856292187035;-5.18730472626354;1.19958413590773]
Robot Arm 1 togoal: 16.9476635925097
Mobile Platform 2 Pose: [2.58373560693239;-3.97567794803883;-9.75042555351532]
Robot Arm 2 end: [-4.46925460142083;-10.4071359463427;0.529282347786982]
Robot Arm 1 togoal: 5.63368513891395

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990532e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-9.84403545088672;1.92864310384654;-5.2859962688099]
Robot Arm 1 end: [2.41419455617938;-5.31545531156138;1.33310121014786]
Robot Arm 1 togoal: 17.0921889056715
Mobile Platform 2 Pose: [2.69779300519748;-4.10192037614399;-9.82464423444947]
Robot Arm 2 end: [-4.72760458613408;-10.3700065929142;0.619057182576137]
Robot Arm 1 togoal: 5.36176828339838

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.605512e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-9.91706790469659;2.06524401770718;-5.17565505935672]
Robot Arm 1 end: [2.33857607953951;-5.29288968916626;1.424249693954]
Robot Arm 1 togoal: 17.1049481775226
Mobile Platform 2 Pose: [2.60302164486978;-4.14667721940087;-9.89804004001116]
Robot Arm 2 end: [-4.63236960209007;-10.5339050590039;0.782717613282104]
Robot Arm 1 togoal: 5.44440736748588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011272e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-9.8592077807286;2.04702853737722;-5.10732832600624]
Robot Arm 1 end: [2.24708959427225;-5.11653729617975;1.47465574658092]
Robot Arm 1 togoal: 16.9888041773357
Mobile Platform 2 Pose: [2.74937738678126;-4.29071220178378;-9.97878821081047]
Robot Arm 2 end: [-4.87724768792315;-10.4880172529907;0.868396796115796]
Robot Arm 1 togoal: 5.18716136550862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.844964e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-9.94141784873013;2.10747010441274;-5.0163389162184]
Robot Arm 1 end: [2.3224833290272;-4.99907602926945;1.47500973772665]
Robot Arm 1 togoal: 16.8498860254037
Mobile Platform 2 Pose: [2.65014013132483;-4.34051985477231;-10.0642249366588]
Robot Arm 2 end: [-4.75404652770063;-10.6124425207376;1.03296609438445]
Robot Arm 1 togoal: 5.30408246238382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.936867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-9.8881040997204;2.08000142525244;-4.94032568622722]
Robot Arm 1 end: [2.18099884233584;-4.84140011463052;1.49933566601908]
Robot Arm 1 togoal: 16.7751126914091
Mobile Platform 2 Pose: [2.89448110174375;-4.55061538643193;-10.1675614677494]
Robot Arm 2 end: [-5.02506972054498;-10.5916132518606;1.10462588854214]
Robot Arm 1 togoal: 5.02725620253313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-10.0992592141633;2.20087387129188;-4.88019367553858]
Robot Arm 1 end: [2.41167117457941;-4.75906491709973;1.47802762093649]
Robot Arm 1 togoal: 16.5956192477182
Mobile Platform 2 Pose: [2.75339819201778;-4.62546064818727;-10.2927990350165]
Robot Arm 2 end: [-4.88717442954965;-10.6941002254584;1.24118420188991]
Robot Arm 1 togoal: 5.16730658251514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998964e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.923632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-9.93876460190399;2.11102175943768;-4.83687335192382]
Robot Arm 1 end: [2.17853357372278;-4.67833130454143;1.49650486925751]
Robot Arm 1 togoal: 16.6321780580504
Mobile Platform 2 Pose: [3.13390224249205;-4.87723929572021;-10.4204727019293]
Robot Arm 2 end: [-5.19945303591204;-10.5807197860423;1.30370494582573]
Robot Arm 1 togoal: 4.84042392408803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.336089e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.822316e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-10.272506192616;2.30259125866712;-4.80445840978908]
Robot Arm 1 end: [2.47038594100832;-4.61114716866341;1.47972447298988]
Robot Arm 1 togoal: 16.4372264427681
Mobile Platform 2 Pose: [3.56686271312207;-5.10650459216505;-10.4723144676178]
Robot Arm 2 end: [-5.53512992937717;-10.488714722025;1.2273529025307]
Robot Arm 1 togoal: 4.50112601966659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-10.0527109129347;2.18607204596249;-4.74367106680004]
Robot Arm 1 end: [2.20770433867439;-4.57526030769587;1.49898618893872]
Robot Arm 1 togoal: 16.5275094711359
Mobile Platform 2 Pose: [3.6618538554995;-5.1513120457499;-10.4791842016221]
Robot Arm 2 end: [-5.49310376165628;-10.5819796064881;1.30208122521407]
Robot Arm 1 togoal: 4.54958672799174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.552018e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-10.5147063597442;2.41711556859767;-4.83677223877504]
Robot Arm 1 end: [2.15018723580508;-4.49462254843269;1.39962258954551]
Robot Arm 1 togoal: 16.4841856253265
Mobile Platform 2 Pose: [4.19805362621958;-5.38860322125541;-10.533447217467]
Robot Arm 2 end: [-5.74851074389905;-10.5256055856368;1.33747826735676]
Robot Arm 1 togoal: 4.2877852503066

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-10.4004904305904;2.35391273353434;-4.84824238463891]
Robot Arm 1 end: [2.16612350605054;-4.57736778962845;1.45537858790534]
Robot Arm 1 togoal: 16.5491262236493
Mobile Platform 2 Pose: [4.36768915512801;-5.46590542671607;-10.5529265724035]
Robot Arm 2 end: [-5.91708133153887;-10.6278502214426;1.28906303952717]
Robot Arm 1 togoal: 4.1374165257401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-10.1165474386161;2.19050487766618;-4.89474250496508]
Robot Arm 1 end: [2.08805025845359;-4.69476202973581;1.48089100261716]
Robot Arm 1 togoal: 16.6894154647487
Mobile Platform 2 Pose: [4.51290859274361;-5.53451658304639;-10.5688354049927]
Robot Arm 2 end: [-5.96900702783814;-10.7532767440905;1.26878951570469]
Robot Arm 1 togoal: 4.1085204542748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989184e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-10.0273456468422;2.13675826227294;-4.86644318539242]
Robot Arm 1 end: [1.99179494310233;-4.68838564604095;1.48870751745021]
Robot Arm 1 togoal: 16.729646258202
Mobile Platform 2 Pose: [4.64951000322915;-5.6008340229425;-10.583722259197]
Robot Arm 2 end: [-5.91998763100613;-10.8476306571128;1.27854704636259]
Robot Arm 1 togoal: 4.17417801450153

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-10.277231084102;2.27692505584639;-4.91720502561472]
Robot Arm 1 end: [2.25889024220565;-4.66396027309637;1.47873475669791]
Robot Arm 1 togoal: 16.5818665271422
Mobile Platform 2 Pose: [4.77916609833058;-5.667729550389;-10.5988917666244]
Robot Arm 2 end: [-5.87127409888888;-10.9363625228838;1.25454565863451]
Robot Arm 1 togoal: 4.24195123225146

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000560e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-10.5704178110383;2.44644968403117;-4.91697209597864]
Robot Arm 1 end: [2.55920298878923;-4.71713394392896;1.47819482673907]
Robot Arm 1 togoal: 16.4912499213506
Mobile Platform 2 Pose: [4.90844328791646;-5.74240782645086;-10.6094969619635]
Robot Arm 2 end: [-5.82239915699233;-11.0824365827839;1.14338230830214]
Robot Arm 1 togoal: 4.33204488438548

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997781e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.963037e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-10.5243157545282;2.41957266328083;-4.93937281731106]
Robot Arm 1 end: [2.52167203315938;-4.60515971232048;1.44726814606782]
Robot Arm 1 togoal: 16.4085805537318
Mobile Platform 2 Pose: [5.07362677067314;-5.86069014029969;-10.6020312463534]
Robot Arm 2 end: [-5.89841028272265;-11.1934289373238;1.06922297400529]
Robot Arm 1 togoal: 4.295510833246

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984067e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.753562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-10.6995412319633;2.51844544841164;-4.93495161326318]
Robot Arm 1 end: [2.54839238876332;-4.7212278740636;1.48992473905875]
Robot Arm 1 togoal: 16.4997567189274
Mobile Platform 2 Pose: [5.17161913747907;-6.07086423311751;-10.5246670705909]
Robot Arm 2 end: [-5.98870010409676;-11.1827524058206;1.00754611815924]
Robot Arm 1 togoal: 4.21343861912396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.650263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-10.8745330350169;2.6177508534109;-4.84483556448147]
Robot Arm 1 end: [2.59617580465564;-4.72720760358131;1.5072418462378]
Robot Arm 1 togoal: 16.4835507643634
Mobile Platform 2 Pose: [4.96341861655323;-6.16404776069967;-10.4479835679415]
Robot Arm 2 end: [-6.1209993134105;-11.1071366031823;0.987128423689641]
Robot Arm 1 togoal: 4.06908056502449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.656113e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-10.9636503787569;2.62309799531418;-4.75719603788624]
Robot Arm 1 end: [2.69868788016853;-4.53948118288081;1.48467069799734]
Robot Arm 1 togoal: 16.2698184242089
Mobile Platform 2 Pose: [4.91739263276123;-6.25697662379765;-10.4503999912471]
Robot Arm 2 end: [-6.24043878156965;-11.1492839297774;0.961854397161442]
Robot Arm 1 togoal: 3.97086865956855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970615e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-10.8331206977269;2.50837080966603;-4.55287515385051]
Robot Arm 1 end: [2.63724274162033;-4.2081350480074;1.42489157931282]
Robot Arm 1 togoal: 16.0028289004402
Mobile Platform 2 Pose: [4.81785217014075;-6.47360138942557;-10.4517347521584]
Robot Arm 2 end: [-6.53540011300148;-11.1143086335413;1.02951739267114]
Robot Arm 1 togoal: 3.67242307762623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971398e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-10.6900421091022;2.42419836287774;-4.53956196831437]
Robot Arm 1 end: [2.44240652959607;-4.09254873643342;1.39241810648101]
Robot Arm 1 togoal: 15.9916754049094
Mobile Platform 2 Pose: [4.73561535287729;-6.76072273252737;-10.4027377826988]
Robot Arm 2 end: [-6.80135134216007;-10.9639728889936;1.17716671344829]
Robot Arm 1 togoal: 3.3583951965123

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.289686e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989541e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-11.0485005688279;2.61903943627621;-4.43215687929434]
Robot Arm 1 end: [2.89056888432747;-4.09870551677609;1.38225879142434]
Robot Arm 1 togoal: 15.7904007916272
Mobile Platform 2 Pose: [4.97295692455609;-6.99945348291903;-10.400771250172]
Robot Arm 2 end: [-7.02088171769157;-10.8532684156544;1.27285580609464]
Robot Arm 1 togoal: 3.10882425847886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.694062e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-10.9215347108259;2.54724835091538;-4.42069471523977]
Robot Arm 1 end: [2.71634085724512;-3.98469062542993;1.37577724905786]
Robot Arm 1 togoal: 15.7684606745305
Mobile Platform 2 Pose: [5.44038736980679;-7.54563421353131;-10.281202037079]
Robot Arm 2 end: [-7.50180043418128;-10.3878112573042;1.42630502273166]
Robot Arm 1 togoal: 2.52989442085407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.175325e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.624045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-11.020135439785;2.62184727492104;-4.03091667252266]
Robot Arm 1 end: [3.07624867438361;-3.65355824384776;1.24426560713841]
Robot Arm 1 togoal: 15.3112562208759
Mobile Platform 2 Pose: [4.94892347653913;-7.34803368289776;-10.0439691813617]
Robot Arm 2 end: [-7.48616977510203;-10.5643624089452;1.08117158397874]
Robot Arm 1 togoal: 2.61367487721423

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.242496e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-11.090532392935;2.65364535273307;-4.04576598791205]
Robot Arm 1 end: [3.04712281119555;-3.61475234761731;1.2596446271337]
Robot Arm 1 togoal: 15.2896137727699
Mobile Platform 2 Pose: [4.77374521999875;-7.28671346477842;-9.85587584938188]
Robot Arm 2 end: [-7.43693468984515;-10.5018110616303;0.809524410506844]
Robot Arm 1 togoal: 2.70690148327167

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-11.1969457018852;2.70506376293337;-4.06388410668106]
Robot Arm 1 end: [3.01908931410459;-3.59515027967959;1.28418661869158]
Robot Arm 1 togoal: 15.2845446680327
Mobile Platform 2 Pose: [4.53872954704003;-7.29269727265961;-9.67066947417584]
Robot Arm 2 end: [-7.25073383293087;-10.2748827871379;0.552364604404493]
Robot Arm 1 togoal: 2.92784554470624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.407031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-11.3139132902841;2.76786498542009;-4.07460472501713]
Robot Arm 1 end: [2.98817759543718;-3.58506732572449;1.3119231258041]
Robot Arm 1 togoal: 15.2893237527668
Mobile Platform 2 Pose: [4.59773618963657;-7.38500337194759;-9.57107542603138]
Robot Arm 2 end: [-7.35106996212931;-10.2312649883016;0.661788230114971]
Robot Arm 1 togoal: 2.79437984280175

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.807597e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.414673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-11.5038618630363;2.87045153493378;-4.07932556655988]
Robot Arm 1 end: [2.98899827434118;-3.58794351617302;1.33965267265176]
Robot Arm 1 togoal: 15.2911491081944
Mobile Platform 2 Pose: [5.00820904229194;-7.58587141917758;-9.36578849140433]
Robot Arm 2 end: [-7.66923557073257;-9.74694073056438;0.58115294403726]
Robot Arm 1 togoal: 2.5258294295513

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.549387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.264073e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-11.6334767717235;2.94490838122823;-4.07900020262796]
Robot Arm 1 end: [2.9678855991576;-3.60785078580881;1.36603545414106]
Robot Arm 1 togoal: 15.3182325992866
Mobile Platform 2 Pose: [4.647774242775;-7.59564084494625;-9.2022219268994]
Robot Arm 2 end: [-7.69420767549467;-9.64304940767791;0.568459238598105]
Robot Arm 1 togoal: 2.52020353766098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.060203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-11.7765763250256;3.0274038861722;-4.07331552272962]
Robot Arm 1 end: [2.96725841642767;-3.63907740692987;1.38921718411978]
Robot Arm 1 togoal: 15.3460500912806
Mobile Platform 2 Pose: [4.08201920606892;-7.39578310235399;-9.10521704823797]
Robot Arm 2 end: [-7.56104493569766;-9.599903712154;0.556700725742092]
Robot Arm 1 togoal: 2.65300809187769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.143292e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-11.9336043461977;3.11719220801943;-4.06135437281795]
Robot Arm 1 end: [2.98849534535685;-3.67361748940304;1.40792300032713]
Robot Arm 1 togoal: 15.366905972786
Mobile Platform 2 Pose: [3.55033780463703;-7.17252378871924;-9.05104796889272]
Robot Arm 2 end: [-7.24599227633411;-9.63806123416638;0.543194888339876]
Robot Arm 1 togoal: 2.94476842020102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.751041e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-12.1020151376777;3.21159575916724;-4.04219895055838]
Robot Arm 1 end: [3.02765360733616;-3.7046454091255;1.42177790179563]
Robot Arm 1 togoal: 15.3766304837545
Mobile Platform 2 Pose: [3.24106666635861;-7.03690652122799;-9.03185458677799]
Robot Arm 2 end: [-7.01805232364656;-9.66579519455978;0.464192296984129]
Robot Arm 1 togoal: 3.18128076686776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.523889e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-12.2776840818724;3.3065484018696;-4.01465009181675]
Robot Arm 1 end: [3.07797831396137;-3.72774400804314;1.43124400498965]
Robot Arm 1 togoal: 15.374439694213
Mobile Platform 2 Pose: [3.29523503336591;-7.05051870034924;-9.01161677189024]
Robot Arm 2 end: [-6.99226106986733;-9.71237772296491;0.51514625608546]
Robot Arm 1 togoal: 3.18448768253692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-12.4538826485812;3.39504148461881;-3.97612332224381]
Robot Arm 1 end: [3.13035341559221;-3.73894757467832;1.43715107384105]
Robot Arm 1 togoal: 15.3609164862271
Mobile Platform 2 Pose: [3.55373889185138;-7.15885458107303;-9.00329257558736]
Robot Arm 2 end: [-7.43214355481822;-9.74309905821037;0.838757483873749]
Robot Arm 1 togoal: 2.66933318785128

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-12.6049922028261;3.45585001705613;-3.923292677347]
Robot Arm 1 end: [3.17738614262978;-3.71188800461629;1.44010618119069]
Robot Arm 1 togoal: 15.3156937912666
Mobile Platform 2 Pose: [4.09858290520005;-7.39524837364338;-9.00098326165172]
Robot Arm 2 end: [-7.92265205176564;-9.4530174603346;1.14766571880652]
Robot Arm 1 togoal: 2.18035384335813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.827056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-12.6802601655956;3.46571383139107;-3.873493908468]
Robot Arm 1 end: [3.23771568817798;-3.6106136998494;1.44086671298686]
Robot Arm 1 togoal: 15.1981484314416
Mobile Platform 2 Pose: [5.02037478957251;-7.74080567924557;-9.02900960898969]
Robot Arm 2 end: [-7.52666109623834;-8.91168181596777;0.97285964799995]
Robot Arm 1 togoal: 2.75722680388228

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.522803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-12.7238154760507;3.44766027999187;-3.79557264791663]
Robot Arm 1 end: [3.27935026851942;-3.49347487247576;1.44189471337819]
Robot Arm 1 togoal: 15.0747223720292
Mobile Platform 2 Pose: [4.93960543788545;-7.76039332520066;-9.03138895770754]
Robot Arm 2 end: [-7.61443180436597;-8.88659645724509;0.935702275937176]
Robot Arm 1 togoal: 2.69688273583278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.184266e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-12.7225693387961;3.40230444766315;-3.69314597813402]
Robot Arm 1 end: [3.29503686192667;-3.36913508310711;1.44413386724409]
Robot Arm 1 togoal: 14.9564772575514
Mobile Platform 2 Pose: [4.85776490622134;-7.86891955731454;-9.05778884219749]
Robot Arm 2 end: [-7.81466751790625;-8.90481768195975;0.896824916520309]
Robot Arm 1 togoal: 2.52283509208963

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.016507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-12.6556236545977;3.32718328272868;-3.58968451305086]
Robot Arm 1 end: [3.28788587894249;-3.26075077672644;1.4490041233145]
Robot Arm 1 togoal: 14.8628789453057
Mobile Platform 2 Pose: [5.12700852140593;-7.97101371856902;-9.01222829992411]
Robot Arm 2 end: [-7.98676822718293;-8.86495194892955;0.958656979036107]
Robot Arm 1 togoal: 2.3785848571253

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.368656e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-12.5471501939656;3.23685774856948;-3.47790549662805]
Robot Arm 1 end: [3.25510467051903;-3.16428760731584;1.45694635750806]
Robot Arm 1 togoal: 14.7917606823157
Mobile Platform 2 Pose: [5.53294222986545;-8.1332612747841;-8.96585671181349]
Robot Arm 2 end: [-8.18237998051392;-9.09293866581677;0.760714381606634]
Robot Arm 1 togoal: 2.16899447227111

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.822209e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-12.4107280098914;3.13945764912391;-3.37845270947938]
Robot Arm 1 end: [3.21020641097067;-3.09465272958616;1.46614816279535]
Robot Arm 1 togoal: 14.750397818843
Mobile Platform 2 Pose: [5.56005949299994;-8.08142594753504;-8.87933741505966]
Robot Arm 2 end: [-8.1721600866632;-9.07001590369871;0.708704689977686]
Robot Arm 1 togoal: 2.20583155269043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.584930e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-12.2585432349909;3.03935524185854;-3.27408012404697]
Robot Arm 1 end: [3.14775478595599;-3.02368740612013;1.47370017855653]
Robot Arm 1 togoal: 14.7163832309039
Mobile Platform 2 Pose: [5.60660415733438;-8.08883345911492;-8.81660575936042]
Robot Arm 2 end: [-8.18170171932344;-9.07300955235351;0.614656242496147]
Robot Arm 1 togoal: 2.23315448061825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.997043e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.707166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-12.1003913992468;2.93940342873098;-3.17455222215545]
Robot Arm 1 end: [3.1708806350165;-3.05056879480622;1.47705516652435]
Robot Arm 1 togoal: 14.7294313643654
Mobile Platform 2 Pose: [5.54332237075394;-8.05172504442736;-8.74957612375107]
Robot Arm 2 end: [-8.08947436030227;-9.04935876742118;0.498029446379189]
Robot Arm 1 togoal: 2.36649439606281

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.333634e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.679248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-11.8923568946135;2.87318866517141;-2.98211427621075]
Robot Arm 1 end: [3.12297752516898;-2.99621243812944;1.47964929913325]
Robot Arm 1 togoal: 14.7036283187784
Mobile Platform 2 Pose: [5.45065986743404;-8.00504553275294;-8.66923828761909]
Robot Arm 2 end: [-8.02044085775486;-9.26375759873132;0.305675781448606]
Robot Arm 1 togoal: 2.43674377546548

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-11.9292072648249;3.01827357565615;-2.77944811211081]
Robot Arm 1 end: [3.15782039309994;-2.70925347673055;1.5021378780602]
Robot Arm 1 togoal: 14.4340188959455
Mobile Platform 2 Pose: [5.0211463454959;-7.99096472970842;-8.45635841379362]
Robot Arm 2 end: [-7.81393606543445;-8.81279790415411;0.398941415347049]
Robot Arm 1 togoal: 2.72898146013901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.647392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.078572e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-11.7529550913982;2.9017331485526;-2.74043757012115]
Robot Arm 1 end: [3.20310758789517;-2.75449562595129;1.45415583484827]
Robot Arm 1 togoal: 14.4526597301865
Mobile Platform 2 Pose: [5.39992033668533;-8.19192483692687;-8.33386635551953]
Robot Arm 2 end: [-8.10444894848678;-9.07706374971394;0.589518867180876]
Robot Arm 1 togoal: 2.30490394849394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.842723e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.404029e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-11.3553386235116;2.70053790639192;-2.59204459677561]
Robot Arm 1 end: [3.11303897392428;-2.69403076496784;1.37991059394546]
Robot Arm 1 togoal: 14.4425951773612
Mobile Platform 2 Pose: [5.79460787676773;-8.0530441435074;-8.20484652855365]
Robot Arm 2 end: [-7.85908632634773;-8.67897788745472;0.513992863525485]
Robot Arm 1 togoal: 2.70973691578638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.344411e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.119883e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-10.7153580869217;2.36129017369326;-2.44958140657898]
Robot Arm 1 end: [2.8479287302571;-2.62494967116579;1.29256379203738]
Robot Arm 1 togoal: 14.5118455321182
Mobile Platform 2 Pose: [5.58667835100128;-7.8090984699607;-8.14172625547845]
Robot Arm 2 end: [-7.17299850325057;-8.03300724496888;0.254367293346398]
Robot Arm 1 togoal: 3.66951718548

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973753e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.989830e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-10.749228930302;2.37763921503026;-2.45590033720126]
Robot Arm 1 end: [2.9771091511403;-2.53836506379375;1.17781874978389]
Robot Arm 1 togoal: 14.3753040913388
Mobile Platform 2 Pose: [6.20254645170461;-8.03274473774782;-8.02528406836574]
Robot Arm 2 end: [-7.32601822766481;-7.92150966464814;0.361771414953192]
Robot Arm 1 togoal: 3.57968036356901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001817e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.943909e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-10.93789303361;2.46385842586212;-2.49560178187186]
Robot Arm 1 end: [3.1438897270904;-2.43496631221251;1.03272471120122]
Robot Arm 1 togoal: 14.208203546466
Mobile Platform 2 Pose: [7.01594663611226;-8.44924957475877;-7.91575073309477]
Robot Arm 2 end: [-7.73097116896103;-7.89530941573701;0.222340083060472]
Robot Arm 1 togoal: 3.35629734550393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.671247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.338010e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-11.6122947206361;2.77622600819304;-2.64740672913962]
Robot Arm 1 end: [3.42002528124118;-2.33527392462021;0.937924256189681]
Robot Arm 1 togoal: 13.9926776357472
Mobile Platform 2 Pose: [8.12335689458378;-8.98568810415983;-7.85843715038162]
Robot Arm 2 end: [-8.24927560570711;-8.00543316863606;0.328486406571172]
Robot Arm 1 togoal: 2.9095397264966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.671883e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-11.7573033466761;2.86885835399056;-2.65022153267322]
Robot Arm 1 end: [3.50117302396846;-2.24683395616066;0.768748310538071]
Robot Arm 1 togoal: 13.8847245784158
Mobile Platform 2 Pose: [9.374516258264;-9.48930094688281;-7.67081477528079]
Robot Arm 2 end: [-9.13150135979909;-8.36244266288765;0.584799547971358]
Robot Arm 1 togoal: 2.07662110331662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000360e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-11.9435312435784;2.98223873681087;-2.65206008695049]
Robot Arm 1 end: [3.57412633212684;-2.17243228285728;0.613590168624912]
Robot Arm 1 togoal: 13.7943232047929
Mobile Platform 2 Pose: [9.70716397241729;-9.67326316106935;-7.59737865995464]
Robot Arm 2 end: [-9.09125477883263;-7.72870061958781;0.0417827987490783]
Robot Arm 1 togoal: 2.85879385072305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000483e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.052382e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-12.0664095048921;3.05648376920771;-2.65105229923035]
Robot Arm 1 end: [3.5426405714085;-2.08817179522705;0.471748889261062]
Robot Arm 1 togoal: 13.7449014559701
Mobile Platform 2 Pose: [8.29077046819856;-8.76272445794019;-7.61365392754727]
Robot Arm 2 end: [-8.78738540399462;-7.52204855581949;-0.216679631225095]
Robot Arm 1 togoal: 3.26039997836709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000232e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.193553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-12.1692635663585;3.11762882905527;-2.64867615155367]
Robot Arm 1 end: [3.47986119077382;-2.01646596161428;0.410796456346949]
Robot Arm 1 togoal: 13.7164214067136
Mobile Platform 2 Pose: [8.79664410065067;-9.21745257502389;-7.46189183781164]
Robot Arm 2 end: [-9.10830312815209;-7.33084058762629;-0.194552133985977]
Robot Arm 1 togoal: 3.2958541236454

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999226e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.011038e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-12.2713092929554;3.17687344262141;-2.64574813922252]
Robot Arm 1 end: [3.4082804322361;-1.9707403329989;0.357647955734088]
Robot Arm 1 togoal: 13.7150566516885
Mobile Platform 2 Pose: [9.70125787098667;-9.8374763338592;-7.29417496414433]
Robot Arm 2 end: [-9.57068691669931;-7.17633774329257;-0.141956927595285]
Robot Arm 1 togoal: 3.30496656662127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.706014e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-12.3708269979806;3.23342344458603;-2.64249604615972]
Robot Arm 1 end: [3.33612713653504;-1.93722658275395;0.339730466952148]
Robot Arm 1 togoal: 13.7222439031719
Mobile Platform 2 Pose: [10.6003455926972;-10.3708472688746;-7.21034789006423]
Robot Arm 2 end: [-9.94229348519393;-7.15648472017879;-0.045541527469353]
Robot Arm 1 togoal: 3.24699269258706

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.888890e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-12.4705797269027;3.2888725246731;-2.63901635429915]
Robot Arm 1 end: [3.34071502728197;-1.92710294600389;0.319074467662356]
Robot Arm 1 togoal: 13.7130042666707
Mobile Platform 2 Pose: [11.5755262584347;-10.9116493007032;-7.0097768174819]
Robot Arm 2 end: [-10.9425864978697;-7.62561651748068;0.283251164744919]
Robot Arm 1 togoal: 2.83869484221219

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.583751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-12.5523741759384;3.33384746245859;-2.63749669162079]
Robot Arm 1 end: [3.40473145645884;-1.91771551383306;0.313363705662265]
Robot Arm 1 togoal: 13.6743517451279
Mobile Platform 2 Pose: [11.7000988725288;-11.0392162862207;-7.07220149070414]
Robot Arm 2 end: [-10.9218901021036;-7.66523916757607;0.244188105213092]
Robot Arm 1 togoal: 2.81624539407468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983561e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.001666e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-12.6175448558577;3.36956236154758;-2.63730856641467]
Robot Arm 1 end: [3.49143800252604;-1.91004377629828;0.315316975679232]
Robot Arm 1 togoal: 13.6258648746947
Mobile Platform 2 Pose: [12.1919281184978;-11.2714283867882;-7.2356191681091]
Robot Arm 2 end: [-11.2034339173121;-7.57866959476966;0.141347648093931]
Robot Arm 1 togoal: 3.03554521243713

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986798e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.662971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-12.6709511840874;3.39884599464951;-2.63768444662551]
Robot Arm 1 end: [3.57939720703904;-1.89094745113192;0.357703166596688]
Robot Arm 1 togoal: 13.5636289772747
Mobile Platform 2 Pose: [12.4915821789141;-11.4509275807673;-7.29352581697313]
Robot Arm 2 end: [-10.968669798804;-7.53234585686739;0.0496673660417887]
Robot Arm 1 togoal: 3.03190663922638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989644e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.783374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-12.7217631812188;3.42661373387416;-2.63845190900679]
Robot Arm 1 end: [3.65889941413515;-1.87146244723855;0.42218181807297]
Robot Arm 1 togoal: 13.5036505683902
Mobile Platform 2 Pose: [12.4506673964853;-11.3517881612415;-7.28921906839677]
Robot Arm 2 end: [-10.8148582188936;-7.62987097408533;0.111854126986773]
Robot Arm 1 togoal: 2.87527348400321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991894e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-12.774442697192;3.45517748881046;-2.63961868749439]
Robot Arm 1 end: [3.73104349954792;-1.8579201924328;0.49831849015858]
Robot Arm 1 togoal: 13.4519873760436
Mobile Platform 2 Pose: [12.0344039241247;-10.9595095689377;-7.5477095684544]
Robot Arm 2 end: [-10.6971875903052;-7.86786515434907;0.149719756660164]
Robot Arm 1 togoal: 2.62915937932425

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.553965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-12.8278175101087;3.48399098797042;-2.6411077708256]
Robot Arm 1 end: [3.76576707674435;-1.8408747472311;0.602242540845297]
Robot Arm 1 togoal: 13.413280366479
Mobile Platform 2 Pose: [12.0746974689669;-10.9660710355659;-7.59187743360553]
Robot Arm 2 end: [-10.696224758194;-7.87763065763645;-0.0376105030756467]
Robot Arm 1 togoal: 2.72368266132908

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.771264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-12.8855034685848;3.51508651919716;-2.64248632448415]
Robot Arm 1 end: [3.80451087702255;-1.83854623222578;0.688897323849192]
Robot Arm 1 togoal: 13.3875934069248
Mobile Platform 2 Pose: [12.0473894865273;-10.9706195603954;-7.60424331421658]
Robot Arm 2 end: [-10.5340369348442;-7.8923905602245;0.0492820150830863]
Robot Arm 1 togoal: 2.62548411260409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993848e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-12.945568669708;3.54747311317402;-2.64374205518874]
Robot Arm 1 end: [3.84517681037695;-1.84471684544405;0.760732667483743]
Robot Arm 1 togoal: 13.3700101743102
Mobile Platform 2 Pose: [11.9361438115344;-10.7643926520356;-7.74968614657734]
Robot Arm 2 end: [-10.4546584496101;-8.03441522330036;0.123788454229586]
Robot Arm 1 togoal: 2.45406559077572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993217e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-13.0070086137206;3.58063028027017;-2.64489716478878]
Robot Arm 1 end: [3.88678476950003;-1.85577134354842;0.820328962739493]
Robot Arm 1 togoal: 13.3574568877229
Mobile Platform 2 Pose: [12.1850275978538;-10.8511466499675;-7.91820397146458]
Robot Arm 2 end: [-10.4152829133244;-8.49602303620744;0.271214083864717]
Robot Arm 1 togoal: 1.9990926546531

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.495790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-13.0679694465616;3.61362067256125;-2.64597482922045]
Robot Arm 1 end: [3.95214942100233;-1.87351402069954;0.850919829188927]
Robot Arm 1 togoal: 13.3418828978876
Mobile Platform 2 Pose: [13.1058514459459;-11.1979516670332;-8.16967931448628]
Robot Arm 2 end: [-11.1554365027612;-8.67496116908075;0.18546162565728]
Robot Arm 1 togoal: 2.20780981171359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.324825e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-13.233851869117;3.69907445995287;-2.64705539661269]
Robot Arm 1 end: [4.00309689846699;-1.8984316685042;0.966296279772372]
Robot Arm 1 togoal: 13.3357871607369
Mobile Platform 2 Pose: [13.3845412961647;-11.3399295399256;-8.21801005066253]
Robot Arm 2 end: [-11.0202319168498;-8.68609053543745;0.0238646379964172]
Robot Arm 1 togoal: 2.23800034347353

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.219529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.188183e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-13.2967993250797;3.73348617707404;-2.6475934940234]
Robot Arm 1 end: [3.98773843974916;-1.93295825123945;1.07009840080266]
Robot Arm 1 togoal: 13.3695958782176
Mobile Platform 2 Pose: [13.8756508713739;-11.5327865780189;-8.38282916449828]
Robot Arm 2 end: [-11.2419000241817;-8.68811935169896;-0.118496421516738]
Robot Arm 1 togoal: 2.43952762263835

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.473999e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.622250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-13.3638133001921;3.77053651936162;-2.64732121500694]
Robot Arm 1 end: [3.9612862678261;-1.97582474815014;1.16100068721291]
Robot Arm 1 togoal: 13.4170056809542
Mobile Platform 2 Pose: [14.0563001312123;-11.6461744783617;-8.26872969861559]
Robot Arm 2 end: [-11.4684619304145;-8.52397084252778;-0.168832878045696]
Robot Arm 1 togoal: 2.68152525076849

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.450874e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-13.435608397999;3.81081237936761;-2.64577257519853]
Robot Arm 1 end: [3.92576686118088;-2.02480657596363;1.23674107851176]
Robot Arm 1 togoal: 13.474905734309
Mobile Platform 2 Pose: [14.1919913583172;-11.6998909928777;-7.85475901111358]
Robot Arm 2 end: [-11.4846994020372;-8.12640595663576;-0.0760092198269447]
Robot Arm 1 togoal: 2.87491311206995

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.687585e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-13.512578213556;3.85467837176706;-2.64227871555434]
Robot Arm 1 end: [3.8840558447212;-2.07692973874866;1.29581053321302]
Robot Arm 1 togoal: 13.5391179594947
Mobile Platform 2 Pose: [14.9317411382575;-12.0190651668697;-7.70422785965747]
Robot Arm 2 end: [-11.7339235359909;-7.90029927083114;0.205351879842924]
Robot Arm 1 togoal: 3.02426254759874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.248002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-13.5943611378893;3.90173820797014;-2.63634164021878]
Robot Arm 1 end: [3.85038476316579;-2.12791291532337;1.33827105332576]
Robot Arm 1 togoal: 13.599170162577
Mobile Platform 2 Pose: [15.3339440836396;-12.2144980729794;-7.6508542569566]
Robot Arm 2 end: [-11.9110758286195;-7.60942634143763;0.406516452203183]
Robot Arm 1 togoal: 3.25716545758055

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.530955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-13.679566866596;3.95055366501272;-2.62895261095068]
Robot Arm 1 end: [3.8112681834538;-2.17605939911125;1.36549284267061]
Robot Arm 1 togoal: 13.6594658191233
Mobile Platform 2 Pose: [15.4352933752872;-12.1747508650651;-7.58436451035761]
Robot Arm 2 end: [-11.8266528620328;-7.47768303256585;0.454629092064841]
Robot Arm 1 togoal: 3.29179137793834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.100406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-13.768476258876;4.00061291890068;-2.62047912354412]
Robot Arm 1 end: [3.77535602823318;-2.22382895998039;1.37937887214205]
Robot Arm 1 togoal: 13.7181720196958
Mobile Platform 2 Pose: [15.4731014566179;-12.1081024803251;-7.5358423672564]
Robot Arm 2 end: [-11.7426482130609;-7.04874149138171;0.806570300609347]
Robot Arm 1 togoal: 3.50102261839989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.963412e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-13.861239766959;4.05151818440132;-2.6107436766964]
Robot Arm 1 end: [3.75232564424495;-2.27051404806536;1.38338940458056]
Robot Arm 1 togoal: 13.7701810639984
Mobile Platform 2 Pose: [15.1709317276562;-11.7762578349437;-7.44861162308613]
Robot Arm 2 end: [-11.6535707294605;-6.84772840113627;0.503718615497759]
Robot Arm 1 togoal: 3.7021579930611

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.407545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-13.9564978255023;4.10199739416718;-2.59940144021021]
Robot Arm 1 end: [3.74774394876835;-2.31345455151479;1.38328354917388]
Robot Arm 1 togoal: 13.8105334633826
Mobile Platform 2 Pose: [14.7357045004112;-11.5274522747447;-7.47312527775823]
Robot Arm 2 end: [-11.5454397184873;-6.8486800817708;0.439377497411689]
Robot Arm 1 togoal: 3.67275215453094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.751736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-14.0516344910938;4.14995934363195;-2.58631105171922]
Robot Arm 1 end: [3.76367943726597;-2.34713873074979;1.38470562405172]
Robot Arm 1 togoal: 13.8333692625649
Mobile Platform 2 Pose: [14.4718367060765;-11.3758160984323;-7.49387615377252]
Robot Arm 2 end: [-11.5197478626658;-6.8461908278778;0.620123157615547]
Robot Arm 1 togoal: 3.61493083549104

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.747849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-14.1435512266209;4.19320919997889;-2.57179970198417]
Robot Arm 1 end: [3.79856988919861;-2.36498354622005;1.39097731163838]
Robot Arm 1 togoal: 13.8335627882674
Mobile Platform 2 Pose: [14.2548700190352;-11.2483726981059;-7.50784429616785]
Robot Arm 2 end: [-11.4712354469869;-6.87434142189327;0.556510857626422]
Robot Arm 1 togoal: 3.5867136201709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981734e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-14.2306066632828;4.23074413234846;-2.55631826918499]
Robot Arm 1 end: [3.84868771672965;-2.36405747698393;1.40239193667959]
Robot Arm 1 togoal: 13.8102363474764
Mobile Platform 2 Pose: [14.3558048408169;-11.3061833080991;-7.50391215087331]
Robot Arm 2 end: [-11.5339528591885;-6.9274839951595;0.413114299393152]
Robot Arm 1 togoal: 3.60843133505719

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.846599e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-14.3142105272802;4.26272069392653;-2.53913217952528]
Robot Arm 1 end: [3.90982588252216;-2.34724593125773;1.41717182892627]
Robot Arm 1 togoal: 13.7679149894698
Mobile Platform 2 Pose: [14.6343708478964;-11.4641040854744;-7.46905017027305]
Robot Arm 2 end: [-11.6424732697448;-7.03063957432592;0.260926990297298]
Robot Arm 1 togoal: 3.6197518933163

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.013878e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-14.395712643682;4.28785901080803;-2.51818075138724]
Robot Arm 1 end: [3.97888656663222;-2.31685705444459;1.43309540651223]
Robot Arm 1 togoal: 13.7100875961702
Mobile Platform 2 Pose: [14.8230167107175;-11.5706861747567;-7.45143962973731]
Robot Arm 2 end: [-11.7069211733228;-7.21204056757114;0.166879385376859]
Robot Arm 1 togoal: 3.5383527703939

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.945776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-14.3629717286477;4.24709340182224;-2.49062728159159]
Robot Arm 1 end: [3.98451931942204;-2.28938451825724;1.44756790608983]
Robot Arm 1 togoal: 13.6828495544022
Mobile Platform 2 Pose: [14.903046238421;-11.6141071282416;-7.43733383018675]
Robot Arm 2 end: [-11.6674003158971;-7.41676803078589;0.107511069031055]
Robot Arm 1 togoal: 3.38397136190529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.994362e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.487132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-14.4336909845204;4.25162642726489;-2.46062991112141]
Robot Arm 1 end: [4.02096921668619;-2.28707269633383;1.45740429736435]
Robot Arm 1 togoal: 13.6647359640134
Mobile Platform 2 Pose: [14.8607125216293;-11.5847366858954;-7.42510457879726]
Robot Arm 2 end: [-11.5431374273182;-7.54899830044117;0.0385767134772875]
Robot Arm 1 togoal: 3.25365352354171

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.746754e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.884065e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-14.5196130168283;4.27092023695939;-2.44059033269576]
Robot Arm 1 end: [4.07141068819261;-2.31992684676711;1.46642532605198]
Robot Arm 1 togoal: 13.6722985314396
Mobile Platform 2 Pose: [14.7339292791284;-11.4865021909583;-7.42859301450934]
Robot Arm 2 end: [-11.3289767976263;-7.70262654015938;-0.0327451944892035]
Robot Arm 1 togoal: 3.07542326713048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998227e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.293278e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-14.6727268491751;4.35287625257444;-2.42391972103777]
Robot Arm 1 end: [4.21184351224305;-2.38913351698463;1.48268456803189]
Robot Arm 1 togoal: 13.6746061332955
Mobile Platform 2 Pose: [14.1834415507467;-11.07772532301;-7.51009189694573]
Robot Arm 2 end: [-10.68405546078;-7.73665590208071;0.00017384992915731]
Robot Arm 1 togoal: 2.81132896647624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.521484e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.101904e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-14.8173793829861;4.46181743353492;-2.29808220275665]
Robot Arm 1 end: [4.36234454319999;-2.29699711857314;1.49116111120098]
Robot Arm 1 togoal: 13.5277561902582
Mobile Platform 2 Pose: [13.9717934168601;-10.7308652369528;-7.63584937138085]
Robot Arm 2 end: [-11.0475322028554;-8.07585649309601;0.0392050436715697]
Robot Arm 1 togoal: 2.64487584657207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.434064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-14.7088675624156;4.43672885089459;-2.15069508199092]
Robot Arm 1 end: [4.41395926109381;-2.07191836538507;1.47231418250827]
Robot Arm 1 togoal: 13.3017831310718
Mobile Platform 2 Pose: [13.7894443655694;-10.3799049867645;-7.54208170533054]
Robot Arm 2 end: [-10.6710650072519;-7.92980927756057;0.46942896821343]
Robot Arm 1 togoal: 2.41698562389703

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993807e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.440224e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-14.6700030007436;4.43211098138357;-2.10631000964358]
Robot Arm 1 end: [4.49723975817868;-2.01253394268189;1.441179562303]
Robot Arm 1 togoal: 13.213164394093
Mobile Platform 2 Pose: [13.8152386002897;-10.5809231700768;-7.41617208569398]
Robot Arm 2 end: [-10.6022857392389;-8.01244427287406;0.514433602219487]
Robot Arm 1 togoal: 2.30787820156579

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.880721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-14.6504543103935;4.42581360932414;-2.09151060749233]
Robot Arm 1 end: [4.5579127606318;-1.97290390035542;1.3939541934672]
Robot Arm 1 togoal: 13.1522956918174
Mobile Platform 2 Pose: [13.368794760048;-10.3342502378364;-7.36958806418166]
Robot Arm 2 end: [-10.0118326195059;-8.01729328223713;0.722637372765534]
Robot Arm 1 togoal: 2.13743978256958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978757e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.381349e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-14.6483819710147;4.4271218941284;-2.08190853405693]
Robot Arm 1 end: [4.59423436494773;-1.91987965436203;1.3292989986934]
Robot Arm 1 togoal: 13.0897892400714
Mobile Platform 2 Pose: [12.9106581428885;-10.0504702409589;-7.28849873467878]
Robot Arm 2 end: [-9.59028839789264;-7.86579692559557;0.937807849020377]
Robot Arm 1 togoal: 2.25006654227446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.507908e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-14.6636165143095;4.43709957636801;-2.0734904132247]
Robot Arm 1 end: [4.63254352667369;-1.87420042357398;1.24815773944921]
Robot Arm 1 togoal: 13.0338278180817
Mobile Platform 2 Pose: [12.0636412879129;-9.34628775972419;-7.03439367818611]
Robot Arm 2 end: [-8.97572442775721;-7.39725374091871;1.19112745263128]
Robot Arm 1 togoal: 2.81642405091323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.343128e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-14.6748170971822;4.44491452710738;-2.06569535279263]
Robot Arm 1 end: [4.66513100754914;-1.84095248712857;1.15254951545787]
Robot Arm 1 togoal: 12.9924877805834
Mobile Platform 2 Pose: [12.198253190293;-9.30492989897272;-6.78950409953432]
Robot Arm 2 end: [-8.95270476600952;-7.15128210864514;1.19872309100361]
Robot Arm 1 togoal: 3.05219320124673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.711562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-14.6933162253079;4.45574737195308;-2.05866416340345]
Robot Arm 1 end: [4.69652658715086;-1.81653417243827;1.04367265924515]
Robot Arm 1 togoal: 12.9609085900423
Mobile Platform 2 Pose: [11.4150028613622;-8.62980709738637;-6.5545290008551]
Robot Arm 2 end: [-8.34724776294623;-6.7100088260174;1.2988192667024]
Robot Arm 1 togoal: 3.68849510779373

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.547333e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-14.7121581133867;4.46502828702559;-2.05321094593614]
Robot Arm 1 end: [4.71798410613203;-1.7933039569271;0.916369383324014]
Robot Arm 1 togoal: 12.9362779927678
Mobile Platform 2 Pose: [11.2885906292715;-8.47594652028811;-6.3552029116681]
Robot Arm 2 end: [-8.26727909371508;-6.41434497565652;1.34137368825677]
Robot Arm 1 togoal: 3.98641559606458

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999470e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.589087e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-14.7118885272173;4.46120688966389;-2.04951200163859]
Robot Arm 1 end: [4.71039497620199;-1.78321719791416;0.781287374443438]
Robot Arm 1 togoal: 12.9372061748052
Mobile Platform 2 Pose: [10.9803580930077;-8.30504764318837;-6.44946960807541]
Robot Arm 2 end: [-8.12939566306539;-6.46088794471384;1.35447380970981]
Robot Arm 1 togoal: 4.00652039771208

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.217601e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-14.678940025802;4.43599855318364;-2.04759688451154]
Robot Arm 1 end: [4.61944995529972;-1.78081400199527;0.653431821488328]
Robot Arm 1 togoal: 12.9803918229573
Mobile Platform 2 Pose: [10.5381827419311;-8.03384693970693;-6.60798331960867]
Robot Arm 2 end: [-7.95959960067182;-6.76066524165273;1.35541161933406]
Robot Arm 1 togoal: 3.83196333203059

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.421953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-14.6638908578976;4.4194509209009;-2.04715501018453]
Robot Arm 1 end: [4.57627465698383;-1.79182856060394;0.557086018888433]
Robot Arm 1 togoal: 13.0151122762016
Mobile Platform 2 Pose: [10.0792749015522;-7.688915288532;-6.70202564832398]
Robot Arm 2 end: [-7.71094382639224;-7.06703145546828;1.16877924607131]
Robot Arm 1 togoal: 3.7371301969081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.004786e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-14.6750100455455;4.41430092132132;-2.04049386079176]
Robot Arm 1 end: [4.60415233628858;-1.83939348971059;0.474226564655178]
Robot Arm 1 togoal: 13.0530511697122
Mobile Platform 2 Pose: [10.0898067915789;-7.64021918825767;-6.73526533198145]
Robot Arm 2 end: [-7.82042802294339;-7.09370409591406;0.943749914860545]
Robot Arm 1 togoal: 3.67835663660699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.530708e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-14.6845773292629;4.41083271063608;-2.02290656978424]
Robot Arm 1 end: [4.66193627618227;-1.84793221603851;0.422901022828774]
Robot Arm 1 togoal: 13.0412516070754
Mobile Platform 2 Pose: [10.1480785148959;-7.66618115060613;-6.77691518517414]
Robot Arm 2 end: [-8.04855354804193;-7.14739183823821;0.874105400525111]
Robot Arm 1 togoal: 3.51624646494518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.098229e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-14.6474841913398;4.38284448086991;-2.00867372037347]
Robot Arm 1 end: [4.70730708364756;-1.87695101278613;0.388692244627218]
Robot Arm 1 togoal: 13.0520758701368
Mobile Platform 2 Pose: [10.241840523267;-7.72395809797782;-6.82057497585754]
Robot Arm 2 end: [-8.27504132561436;-7.18458932026164;0.876208575487565]
Robot Arm 1 togoal: 3.36419020048627

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.996265e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-14.5688389137633;4.33557205954108;-1.99078392188728]
Robot Arm 1 end: [4.7181533745533;-1.85421918866691;0.377633543546598]
Robot Arm 1 togoal: 13.0279585200999
Mobile Platform 2 Pose: [10.5107237894613;-7.88259463573113;-6.90538756623192]
Robot Arm 2 end: [-8.52446587779766;-7.28759878990593;0.888355324949104]
Robot Arm 1 togoal: 3.15191379247172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.625181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-14.4483662864375;4.26257078201401;-1.97740607196412]
Robot Arm 1 end: [4.67920853101767;-1.98521808587763;0.306885745764043]
Robot Arm 1 togoal: 13.169295647254
Mobile Platform 2 Pose: [11.1816150069089;-8.23423016444655;-7.05767359159092]
Robot Arm 2 end: [-8.86583321190188;-7.52983483387699;0.809733948850111]
Robot Arm 1 togoal: 2.80961734879871

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.160249e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-14.2368041317885;4.14340535839036;-1.95445471581726]
Robot Arm 1 end: [4.60355544416364;-2.12579280229097;0.305800798052804]
Robot Arm 1 togoal: 13.3279095896063
Mobile Platform 2 Pose: [11.8327309835348;-8.58373009186656;-7.15015206546059]
Robot Arm 2 end: [-9.30614779090373;-7.46067234281557;0.607216329476411]
Robot Arm 1 togoal: 2.78650613405482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.155109e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-14.0412408010493;4.0483289511669;-1.92353409341228]
Robot Arm 1 end: [4.4931005772575;-2.129939131201;0.2365926089926]
Robot Arm 1 togoal: 13.3832382824531
Mobile Platform 2 Pose: [11.9856925569482;-8.66004959014272;-7.15843113057534]
Robot Arm 2 end: [-9.29245880586557;-7.63897601106086;0.528777027050208]
Robot Arm 1 togoal: 2.65698235707213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.816446e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-13.8827573286485;3.97565731483804;-1.90064292982889]
Robot Arm 1 end: [4.47439052029784;-2.13531103249642;0.232311544872191]
Robot Arm 1 togoal: 13.3962252859521
Mobile Platform 2 Pose: [12.3795470541681;-8.86332503587976;-7.18465903419867]
Robot Arm 2 end: [-9.54986877036255;-7.52377972708689;0.459953411401146]
Robot Arm 1 togoal: 2.73131924663853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973497e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.379463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-13.7387650630739;3.9034632023181;-1.88913384267695]
Robot Arm 1 end: [4.49768545943522;-2.12991514185468;0.301194735141291]
Robot Arm 1 togoal: 13.3752843534664
Mobile Platform 2 Pose: [12.4857295232184;-8.914292728699;-7.18986009883613]
Robot Arm 2 end: [-9.45490888834038;-7.66365085252375;0.373157979373022]
Robot Arm 1 togoal: 2.65954751099512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994605e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.492562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-13.5978544004473;3.83502385566323;-1.87862419982569]
Robot Arm 1 end: [4.49398455210307;-2.18545148712287;0.427580532600707]
Robot Arm 1 togoal: 13.4162960681849
Mobile Platform 2 Pose: [12.7719109246578;-9.06364523929074;-7.20930654529416]
Robot Arm 2 end: [-9.45647111450989;-7.79963629008385;0.370437958780258]
Robot Arm 1 togoal: 2.54181374533497

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000451e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.744921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-13.4578119775837;3.77316540275349;-1.86832145605219]
Robot Arm 1 end: [4.46159954509627;-2.22775436414497;0.550520706039145]
Robot Arm 1 togoal: 13.4585915477345
Mobile Platform 2 Pose: [13.3866660219897;-9.34064900511725;-7.27537102890244]
Robot Arm 2 end: [-9.85523369957657;-7.81623014873225;0.42607550413067]
Robot Arm 1 togoal: 2.44717545278204

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.741234e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.859573e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-13.3152740339468;3.71683550515202;-1.85871102115293]
Robot Arm 1 end: [4.40844870443011;-2.25885108666277;0.676871776831982]
Robot Arm 1 togoal: 13.5002751199044
Mobile Platform 2 Pose: [13.8353187263748;-9.57174441524214;-7.31215720354171]
Robot Arm 2 end: [-10.104156796925;-7.82324220508319;0.391029365945975]
Robot Arm 1 togoal: 2.45478242822072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.086297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.970852e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-13.1728099722667;3.67445774984873;-1.85134150061494]
Robot Arm 1 end: [4.34983327511503;-2.27111557224583;0.801051179763169]
Robot Arm 1 togoal: 13.5285988820044
Mobile Platform 2 Pose: [14.2374093900737;-9.77486325573245;-7.3400319942482]
Robot Arm 2 end: [-10.2138433827856;-7.89370980545023;0.348966876951309]
Robot Arm 1 togoal: 2.41988615797127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.141053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.170224e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-13.1392002747767;3.70357249039776;-1.85826402048599]
Robot Arm 1 end: [4.42552941872859;-2.17887159118896;0.790971041133013]
Robot Arm 1 togoal: 13.4138949517623
Mobile Platform 2 Pose: [14.8028873895565;-10.0400621367244;-7.39182395821565]
Robot Arm 2 end: [-10.5131645619372;-7.92000685264655;0.350841472094822]
Robot Arm 1 togoal: 2.44110227993314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.601226e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-13.1627475822968;3.73255549910565;-1.86321698138135]
Robot Arm 1 end: [4.50175243848457;-2.0340599816051;0.721837409534816]
Robot Arm 1 togoal: 13.2547345026062
Mobile Platform 2 Pose: [15.2607799447522;-10.2718955868749;-7.42866791096622]
Robot Arm 2 end: [-10.6510576504676;-8.01044451082744;0.321835847179662]
Robot Arm 1 togoal: 2.41250943503614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.216312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-13.2155990636807;3.77556862434252;-1.86708295094887]
Robot Arm 1 end: [4.55505383765753;-1.88319362973026;0.633569634960488]
Robot Arm 1 togoal: 13.1013458165665
Mobile Platform 2 Pose: [15.8912563571558;-10.5517823830959;-7.49341422396793]
Robot Arm 2 end: [-11.0510485408366;-7.98333848064062;0.325777982205226]
Robot Arm 1 togoal: 2.56908201322595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.048015e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-13.2832161519126;3.82366519807286;-1.86945417596205]
Robot Arm 1 end: [4.57667535652481;-1.7375208741806;0.551445576259825]
Robot Arm 1 togoal: 12.9661822538843
Mobile Platform 2 Pose: [16.2102702141018;-10.7118241720742;-7.51528821016003]
Robot Arm 2 end: [-11.0542214503917;-8.07658855370783;0.264007616648476]
Robot Arm 1 togoal: 2.52802775103387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.141436e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-13.3597603016352;3.87436741445427;-1.87063526340792]
Robot Arm 1 end: [4.57277596651497;-1.60410296683894;0.48655207853304]
Robot Arm 1 togoal: 12.8522390608355
Mobile Platform 2 Pose: [16.838728832868;-10.9897463054092;-7.58229794759903]
Robot Arm 2 end: [-11.4349261305361;-8.0686624008075;0.263744755069424]
Robot Arm 1 togoal: 2.71473178657772

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.963227e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-13.4422869535964;3.92654441821872;-1.87094574044036]
Robot Arm 1 end: [4.55338786450405;-1.48285365370782;0.443034942867203]
Robot Arm 1 togoal: 12.7547450171065
Mobile Platform 2 Pose: [17.1797752115465;-11.1599440276327;-7.60775605089446]
Robot Arm 2 end: [-11.4511822436737;-8.15400577450013;0.206006136887568]
Robot Arm 1 togoal: 2.69125127867242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.144146e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-13.526337819456;3.97773372659311;-1.87069112834459]
Robot Arm 1 end: [4.51540905259066;-1.37352628891108;0.425081065529597]
Robot Arm 1 togoal: 12.6743392891261
Mobile Platform 2 Pose: [17.8675063943731;-11.446766350592;-7.68373652339165]
Robot Arm 2 end: [-11.8674581303482;-8.14371375972145;0.211488235691364]
Robot Arm 1 togoal: 2.94075152559917

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.125143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-13.6163534792346;4.03111195080324;-1.86992563106206]
Robot Arm 1 end: [4.46337556558041;-1.27920304937756;0.420578691797243]
Robot Arm 1 togoal: 12.6129123763089
Mobile Platform 2 Pose: [18.2190050357776;-11.6202828562659;-7.71028864356799]
Robot Arm 2 end: [-11.8846260975643;-8.20754354464902;0.140584901317383]
Robot Arm 1 togoal: 2.94453078038261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.122140e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-13.7130707898697;4.08745680226722;-1.86831711733285]
Robot Arm 1 end: [4.39967583658737;-1.20283125112671;0.424791714468969]
Robot Arm 1 togoal: 12.5725387802858
Mobile Platform 2 Pose: [18.9268525838031;-11.9062627321445;-7.7880609704111]
Robot Arm 2 end: [-12.2773880876408;-8.21015764212654;0.139790984052896]
Robot Arm 1 togoal: 3.20901394722738

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.174414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-13.8157810911367;4.14651481697725;-1.86539257379268]
Robot Arm 1 end: [4.32696866267275;-1.14750208284895;0.435091384172205]
Robot Arm 1 togoal: 12.5550501696866
Mobile Platform 2 Pose: [19.364364053571;-12.1186087826796;-7.82454598866716]
Robot Arm 2 end: [-12.3546187067661;-8.24365816341002;0.0651165654512219]
Robot Arm 1 togoal: 3.27850003449787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.246421e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-13.9239588734035;4.20793025609578;-1.86058446726269]
Robot Arm 1 end: [4.24930365225078;-1.11554837244204;0.448960711714074]
Robot Arm 1 togoal: 12.5608595382539
Mobile Platform 2 Pose: [20.0539893912151;-12.3933213087211;-7.90354016808496]
Robot Arm 2 end: [-12.6996800301025;-8.26149411405624;0.0623891598628888]
Robot Arm 1 togoal: 3.52678615220156

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.201877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-14.0375947238638;4.27137003002725;-1.85320539320782]
Robot Arm 1 end: [4.17186414920916;-1.10765427028851;0.464335264400446]
Robot Arm 1 togoal: 12.5882362246262
Mobile Platform 2 Pose: [20.3818335077939;-12.5516246791855;-7.93523280774601]
Robot Arm 2 end: [-12.719874731478;-8.23921781219481;-0.0207833933995212]
Robot Arm 1 togoal: 3.5881985034258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.438971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-14.1378010748001;4.32668926381939;-1.84233513895342]
Robot Arm 1 end: [4.09029276403978;-1.12239037291653;0.479725948592798]
Robot Arm 1 togoal: 12.6378977236106
Mobile Platform 2 Pose: [20.9374478576241;-12.791210185773;-8.02953518209963]
Robot Arm 2 end: [-12.9564982964969;-8.27787650090088;-0.0495835399838001]
Robot Arm 1 togoal: 3.76474755852325

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.079061e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.172632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-14.2332503900138;4.3768014712127;-1.82661019578837]
Robot Arm 1 end: [4.02164104268645;-1.17281405576738;0.417145084293834]
Robot Arm 1 togoal: 12.7197109016743
Mobile Platform 2 Pose: [21.4671529645414;-13.0282226828548;-8.13644307878062]
Robot Arm 2 end: [-13.0778861953669;-8.31516307075602;-0.0904047037381752]
Robot Arm 1 togoal: 3.86117650839792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.716722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.420723e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-14.2732610104327;4.39463397224546;-1.80882806618813]
Robot Arm 1 end: [3.90803183342693;-1.32477904192099;0.272219573630494]
Robot Arm 1 togoal: 12.9198354882776
Mobile Platform 2 Pose: [22.0082192301143;-13.1356629784391;-8.43852977014926]
Robot Arm 2 end: [-13.2000193671811;-8.49190653537961;-0.116978543133198]
Robot Arm 1 togoal: 3.89838986712845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.289312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-14.2279543297838;4.38092379987387;-1.81905523112542]
Robot Arm 1 end: [3.96980812505748;-1.3280175480843;0.233400523472841]
Robot Arm 1 togoal: 12.8974845621261
Mobile Platform 2 Pose: [22.0755605813382;-13.1793601952378;-8.53585662155459]
Robot Arm 2 end: [-13.2055232772224;-8.41428805538073;-0.115075340159712]
Robot Arm 1 togoal: 3.93276040088914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990842e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.896874e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-14.2031898036241;4.37151655800379;-1.82218584187687]
Robot Arm 1 end: [4.06211379552457;-1.34553369496864;0.184485462190916]
Robot Arm 1 togoal: 12.8750106217721
Mobile Platform 2 Pose: [21.7732598670743;-13.0069553229903;-8.60593917939108]
Robot Arm 2 end: [-12.968826034402;-8.3295848788074;-0.0668627124291746]
Robot Arm 1 togoal: 3.75839363201557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954676e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.042462e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-14.1352781521525;4.33572317201777;-1.82046539865988]
Robot Arm 1 end: [4.03953355844645;-1.4374845091795;0.132348677365487]
Robot Arm 1 togoal: 12.971952998468
Mobile Platform 2 Pose: [21.8569384977996;-13.0473630455246;-8.59428934659739]
Robot Arm 2 end: [-13.1309384718624;-8.19827945557888;-0.0332945065427063]
Robot Arm 1 togoal: 3.93253150627506

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986422e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.173417e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-14.0685636683822;4.29572297250209;-1.81704332028831]
Robot Arm 1 end: [4.04084824003419;-1.4941257974543;0.108755273459756]
Robot Arm 1 togoal: 13.0238570597826
Mobile Platform 2 Pose: [22.103922707188;-13.1694646407202;-8.56163123937618]
Robot Arm 2 end: [-13.4044763156107;-8.08414845712092;0.0314754011796959]
Robot Arm 1 togoal: 4.18086470103937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998568e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.122680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-14.0758941246814;4.2933951517659;-1.80855828548886]
Robot Arm 1 end: [4.11408356475132;-1.53484104531405;0.0961437202210768]
Robot Arm 1 togoal: 13.0279233050175
Mobile Platform 2 Pose: [22.5921609050932;-13.4009437362255;-8.47549168202478]
Robot Arm 2 end: [-13.8207680192772;-7.97937247948262;0.129039723104734]
Robot Arm 1 togoal: 4.54078817456377

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963160e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.778610e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-14.1272611444893;4.31160563802705;-1.79456285737151]
Robot Arm 1 end: [4.23259662624944;-1.58110442734042;0.0536001623729658]
Robot Arm 1 togoal: 13.0206829204916
Mobile Platform 2 Pose: [23.049632734792;-13.6178333841119;-8.37258165993943]
Robot Arm 2 end: [-14.1439892663267;-7.84271023160267;0.26044678967566]
Robot Arm 1 togoal: 4.83896069216536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.859188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-14.2426246650104;4.36336186615344;-1.77351781143643]
Robot Arm 1 end: [4.37259858180965;-1.5561395843799;0.0468378526152257]
Robot Arm 1 togoal: 12.9377417987802
Mobile Platform 2 Pose: [23.2335340772457;-13.7087058780448;-8.3035428456591]
Robot Arm 2 end: [-14.2166147665211;-7.69187872046904;0.383437702808127]
Robot Arm 1 togoal: 4.93976840654095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000511e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-14.2291290626014;4.34829301118085;-1.7451298568583]
Robot Arm 1 end: [4.43729272712828;-1.52256200654929;0.0378572797671344]
Robot Arm 1 togoal: 12.8807165843758
Mobile Platform 2 Pose: [23.2535962185857;-13.718633483567;-8.29849849158391]
Robot Arm 2 end: [-14.1728802981862;-7.63520080656142;0.456884678482678]
Robot Arm 1 togoal: 4.91299772295554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-14.1705232650511;4.30648691650623;-1.70601718383244]
Robot Arm 1 end: [4.5249008512963;-1.43059614795166;0.0257795191733023]
Robot Arm 1 togoal: 12.7620892988565
Mobile Platform 2 Pose: [23.2367909757936;-13.7090309180819;-8.29796411635876]
Robot Arm 2 end: [-14.1296616192751;-7.60924559315461;0.512935274252384]
Robot Arm 1 togoal: 4.87708993302981

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000554e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-13.8876092727557;4.14030103083834;-1.67139196191685]
Robot Arm 1 end: [4.38510209262479;-1.38810031255399;-0.00325308899962523]
Robot Arm 1 togoal: 12.788246741378
Mobile Platform 2 Pose: [23.2103313732139;-13.6943665681643;-8.29794162333607]
Robot Arm 2 end: [-14.0835628018153;-7.59502910442848;0.556982537605144]
Robot Arm 1 togoal: 4.83618649689169

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001946e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-13.5436801179317;3.94003024577513;-1.66733360933923]
Robot Arm 1 end: [4.19387661620531;-1.36391922764421;-0.0256295119539399]
Robot Arm 1 togoal: 12.8546408665321
Mobile Platform 2 Pose: [23.1794184742023;-13.6774121954839;-8.2980652751542]
Robot Arm 2 end: [-14.0350059585545;-7.58811786328719;0.59066919216667]
Robot Arm 1 togoal: 4.79207303147273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989502e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-13.2608916653356;3.76947044451219;-1.69834424758993]
Robot Arm 1 end: [4.07414302846652;-1.3962421750614;-0.0350890526883732]
Robot Arm 1 togoal: 12.9387607482176
Mobile Platform 2 Pose: [23.1457515233891;-13.6590772398385;-8.29830806557331]
Robot Arm 2 end: [-13.984844074428;-7.5864180284553;0.614419470732234]
Robot Arm 1 togoal: 4.74618251723603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989405e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-13.0360739961069;3.60225901161091;-1.78366051096922]
Robot Arm 1 end: [3.89756789878116;-1.51525720417072;-0.0782943680479175]
Robot Arm 1 togoal: 13.1300635376932
Mobile Platform 2 Pose: [23.1099718041793;-13.6397092983547;-8.29871636531]
Robot Arm 2 end: [-13.9339823185396;-7.58853943205542;0.627800306668997]
Robot Arm 1 togoal: 4.69991113165988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952843e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-13.0819354906328;3.56854634233221;-1.89471007545062]
Robot Arm 1 end: [3.92107908430346;-1.52826235624055;-0.0313426978178771]
Robot Arm 1 togoal: 13.1249335352631
Mobile Platform 2 Pose: [23.0723433069905;-13.6194564745373;-8.29935530675439]
Robot Arm 2 end: [-13.8832014968755;-7.59340129343559;0.629653469357397]
Robot Arm 1 togoal: 4.65461811940168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.043580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-13.0143862466433;3.43745709067195;-2.08799530910209]
Robot Arm 1 end: [3.91137331468954;-1.81429222959197;0.100977358582882]
Robot Arm 1 togoal: 13.3665754814541
Mobile Platform 2 Pose: [23.0329329557576;-13.598362671159;-8.30029405285956]
Robot Arm 2 end: [-13.8329469791062;-7.60034880116445;0.618211553423426]
Robot Arm 1 togoal: 4.61138107295248

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003738e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-12.965720594571;3.38963376582785;-2.10528762618485]
Robot Arm 1 end: [3.77231013009671;-1.83744397946823;0.0729720012521546]
Robot Arm 1 togoal: 13.4538465194123
Mobile Platform 2 Pose: [22.9915552565054;-13.5763358037949;-8.30159970704124]
Robot Arm 2 end: [-13.7831204036033;-7.6096167090997;0.591400655606054]
Robot Arm 1 togoal: 4.57056749284735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-12.9224595677033;3.35202929394767;-2.11398831959203]
Robot Arm 1 end: [3.66954109168209;-1.85357455391448;0.0603095864605179]
Robot Arm 1 togoal: 13.5172318238357
Mobile Platform 2 Pose: [22.9475729499496;-13.553036863148;-8.30333280786715]
Robot Arm 2 end: [-13.732989555319;-7.6229041559627;0.547424247626504]
Robot Arm 1 togoal: 4.53140656060232

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-12.8388253811308;3.2874488346256;-2.12593284266391]
Robot Arm 1 end: [3.52859850289409;-1.84621011137132;0.0650466177623908]
Robot Arm 1 togoal: 13.5768748830778
Mobile Platform 2 Pose: [22.8996446744502;-13.5277384056316;-8.30554754649622]
Robot Arm 2 end: [-13.6812817500739;-7.64368193787399;0.485437735293792]
Robot Arm 1 togoal: 4.49182138307077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-12.7151524328785;3.20352256955099;-2.13843669684607]
Robot Arm 1 end: [3.32176839710291;-1.75867657361659;0.164930617786477]
Robot Arm 1 togoal: 13.5905914615307
Mobile Platform 2 Pose: [22.8454689215923;-13.4991780031885;-8.30830519735419]
Robot Arm 2 end: [-13.6263490208403;-7.67707436868183;0.405902336835087]
Robot Arm 1 togoal: 4.44857657099213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-12.5955951506534;3.13042992274852;-2.14769732645757]
Robot Arm 1 end: [3.14415437794708;-1.72914858357812;0.272552829914521]
Robot Arm 1 togoal: 13.6430995754473
Mobile Platform 2 Pose: [22.7813962013978;-13.4653308230872;-8.31170742199114]
Robot Arm 2 end: [-13.5646262422692;-7.7271963095225;0.314499493315018]
Robot Arm 1 togoal: 4.39634397370744

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.632729e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-12.4870659702647;3.06383886280688;-2.15355417913821]
Robot Arm 1 end: [3.05965105431603;-1.70552456710836;0.403845937345691]
Robot Arm 1 togoal: 13.6541488961685
Mobile Platform 2 Pose: [22.7014961998515;-13.4228447100821;-8.31588400344748]
Robot Arm 2 end: [-13.465966346962;-7.75429439370798;0.27168219331692]
Robot Arm 1 togoal: 4.314731900188

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.153971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-12.2965130022748;2.9436804750467;-2.15275099785977]
Robot Arm 1 end: [2.91526465827941;-1.68389946830095;0.515988508399289]
Robot Arm 1 togoal: 13.7009864515603
Mobile Platform 2 Pose: [22.612493234403;-13.3751846823537;-8.32025596666428]
Robot Arm 2 end: [-13.4089558777467;-7.79757473639083;0.225536301345449]
Robot Arm 1 togoal: 4.26026799776015

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.988748e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-11.9924066246565;2.75213885133565;-2.13384341484803]
Robot Arm 1 end: [2.68298953124553;-1.61787106008185;0.538432338299532]
Robot Arm 1 togoal: 13.765137463112
Mobile Platform 2 Pose: [22.5869759468253;-13.3638302062081;-8.32955288725143]
Robot Arm 2 end: [-13.3663480949834;-7.8870091770521;0.171680112832843]
Robot Arm 1 togoal: 4.19734366860343

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.111365e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-11.779621727325;2.61037961219009;-2.09580297193494]
Robot Arm 1 end: [2.56383051790009;-1.624450255286;0.655865696634408]
Robot Arm 1 togoal: 13.826529487438
Mobile Platform 2 Pose: [22.6258456724621;-13.391944790542;-8.35476520966512]
Robot Arm 2 end: [-13.3513744863019;-8.02118305914807;0.118062984413233]
Robot Arm 1 togoal: 4.13710764937791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.171875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-11.5696845488099;2.48535184139078;-2.07988006712696]
Robot Arm 1 end: [2.3956484383722;-1.62060582796406;0.716655237173823]
Robot Arm 1 togoal: 13.9108451568993
Mobile Platform 2 Pose: [22.6367253818356;-13.4036981295344;-8.37366381902829]
Robot Arm 2 end: [-13.3354869732676;-8.15348916177608;0.0881435536282226]
Robot Arm 1 togoal: 4.07285565916909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.425511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-11.3842800731435;2.37685053006803;-2.070139625338]
Robot Arm 1 end: [2.19623279230817;-1.62772716873645;0.779558565490806]
Robot Arm 1 togoal: 14.0232862462129
Mobile Platform 2 Pose: [22.5239850119683;-13.3396004429724;-8.38669695794897]
Robot Arm 2 end: [-13.2104534975938;-8.19955064582861;0.114115003132231]
Robot Arm 1 togoal: 3.94055261753479
Step 313:
Mobile Platform 1 Pose: [-11.2305209483258;2.2868188350966;-2.06317689739715]
Robot Arm 1 end: [2.02880931476275;-1.6807899772459;0.858780094871731]
Robot Arm 1 togoal: 14.1569513185463
Mobile Platform 2 Pose: [22.1029174634222;-13.0903665335679;-8.40625350170261]
Robot Arm 2 end: [-12.9343518227367;-8.19738256866619;0.160555568109213]
Robot Arm 1 togoal: 3.70279074153661
Step 314:
Mobile Platform 1 Pose: [-11.105615112331;2.21324381649521;-2.0563532138627]
Robot Arm 1 end: [1.95781927326339;-1.72389374521969;0.947179537895159]
Robot Arm 1 togoal: 14.2286902177757
Mobile Platform 2 Pose: [21.6653778384838;-12.8411637102959;-8.4262022583735]
Robot Arm 2 end: [-12.6637339178202;-8.21218654490626;0.217193307266169]
Robot Arm 1 togoal: 3.46289867411354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.616348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-10.9907319427972;2.14536387114561;-2.04423272616546]
Robot Arm 1 end: [1.93010918309877;-1.75928247267004;1.02769956323594]
Robot Arm 1 togoal: 14.270501301274
Mobile Platform 2 Pose: [21.1506137632449;-12.535290278924;-8.44364998740977]
Robot Arm 2 end: [-12.323673015341;-8.36527898498142;0.547445470404482]
Robot Arm 1 togoal: 3.00326114436677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.324648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-10.9465458780646;2.11763083801491;-2.02129682011197]
Robot Arm 1 end: [2.04173051936247;-1.7224589215731;1.01378431249164]
Robot Arm 1 togoal: 14.1777065863302
Mobile Platform 2 Pose: [20.455027573577;-12.0763158300452;-8.48144182805786]
Robot Arm 2 end: [-11.8590341010827;-8.46716528903636;0.793438553856873]
Robot Arm 1 togoal: 2.51692981587436

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.394354e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-10.9653189604974;2.12917929334408;-2.00602545779522]
Robot Arm 1 end: [2.12704810862729;-1.60894558454477;1.02543534876446]
Robot Arm 1 togoal: 14.0355467792439
Mobile Platform 2 Pose: [20.2125677752179;-11.8622339751347;-8.55667900123819]
Robot Arm 2 end: [-12.2381157410124;-8.70144028633763;0.822051235451084]
Robot Arm 1 togoal: 2.68028893509239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.497658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-10.9809585109357;2.13631961120337;-1.99267291439807]
Robot Arm 1 end: [2.21217438823186;-1.54889089228779;1.05410802227125]
Robot Arm 1 togoal: 13.9371839594714
Mobile Platform 2 Pose: [20.1204289964637;-11.7725595253849;-8.58557159638692]
Robot Arm 2 end: [-12.1245144339089;-8.79042203234119;0.978215071245712]
Robot Arm 1 togoal: 2.50424757534388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-10.9782796090149;2.13270661553969;-1.97939058847479]
Robot Arm 1 end: [2.29254163980949;-1.53398769617615;1.09972451334225]
Robot Arm 1 togoal: 13.8785899710767
Mobile Platform 2 Pose: [20.3026078011063;-11.7837201173581;-8.64216865421722]
Robot Arm 2 end: [-12.1063056417404;-8.84952881460596;1.03281286404751]
Robot Arm 1 togoal: 2.44917008077404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.728555e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [-10.9487011908787;2.11465300763348;-1.96557134876352]
Robot Arm 1 end: [2.34701206574956;-1.5524135487079;1.15555358134506]
Robot Arm 1 togoal: 13.8621799947577
Mobile Platform 2 Pose: [20.5025432599248;-11.8160910167878;-8.67789978514085]
Robot Arm 2 end: [-12.0695807376404;-8.90217128635758;1.09186935199625]
Robot Arm 1 togoal: 2.38171060944985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.199023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [-10.8973655672684;2.08440814313268;-1.95423637071197]
Robot Arm 1 end: [2.36640314018436;-1.63883641275921;1.23240853358393]
Robot Arm 1 togoal: 13.9218389265084
Mobile Platform 2 Pose: [20.8537772614203;-11.8942321129088;-8.71763196824175]
Robot Arm 2 end: [-12.029718682039;-8.84586397703536;1.11261436163318]
Robot Arm 1 togoal: 2.37035160205131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.060314e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [-10.8290567354189;2.04347145276534;-1.94624690938345]
Robot Arm 1 end: [2.38510695810215;-1.79173990821936;1.23294607418964]
Robot Arm 1 togoal: 14.0397543129078
Mobile Platform 2 Pose: [21.2460975475283;-11.9555172322966;-8.79771476975237]
Robot Arm 2 end: [-12.0344291897785;-8.86971751607545;1.15346949945224]
Robot Arm 1 togoal: 2.35616622726888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.189411e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [-10.7098999434529;1.97112376486837;-1.93903775269007]
Robot Arm 1 end: [2.33948385090277;-1.94009353827253;1.17337651904306]
Robot Arm 1 togoal: 14.190496237193
Mobile Platform 2 Pose: [21.3623626999086;-11.8457034261968;-8.91093947862596]
Robot Arm 2 end: [-11.6861884634652;-8.67396217212756;1.06114622776744]
Robot Arm 1 togoal: 2.1938717708901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.827351e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.601048e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [-10.4970822907187;1.83497027879055;-1.93332901879706]
Robot Arm 1 end: [2.17937140677637;-2.05942725913298;1.10166929716328]
Robot Arm 1 togoal: 14.3794247329234
Mobile Platform 2 Pose: [20.7924286981488;-11.490305643765;-8.9353809420555]
Robot Arm 2 end: [-11.1600566572045;-8.82645524171687;0.821193847901922]
Robot Arm 1 togoal: 1.79239153036735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.858831e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.259701e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [-10.1494625958939;1.59417780781036;-1.92485379028208]
Robot Arm 1 end: [1.82529018537484;-2.24294433040284;1.01149112222107]
Robot Arm 1 togoal: 14.7300769083942
Mobile Platform 2 Pose: [20.1401398711002;-11.0062944877504;-8.9440781835148]
Robot Arm 2 end: [-10.6746162890992;-8.84362723594467;0.748780372682309]
Robot Arm 1 togoal: 1.5455187672046

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.035348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.341821e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [-10.0907951859779;1.50037885959712;-1.82391814539167]
Robot Arm 1 end: [1.72392515732482;-2.21746344144888;1.00877382072255]
Robot Arm 1 togoal: 14.7655749432062
Mobile Platform 2 Pose: [19.7128180538304;-10.5672767337032;-8.94805706469271]
Robot Arm 2 end: [-10.3877195806928;-8.7415765695798;0.50825428837744]
Robot Arm 1 togoal: 1.66120735607962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.823548e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.980517e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [-10.0267009152757;1.46314502209378;-1.77970934495156]
Robot Arm 1 end: [1.67579659174531;-2.27137984683375;0.956003899158137]
Robot Arm 1 togoal: 14.8390817209571
Mobile Platform 2 Pose: [18.7419761587002;-10.0290592537033;-8.91399651821905]
Robot Arm 2 end: [-9.88659477798151;-8.68755562535443;0.406690340357947]
Robot Arm 1 togoal: 1.7254150221009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [-10.010427556591;1.45783617576835;-1.77032457489918]
Robot Arm 1 end: [1.73902233798081;-2.25226466896638;0.856425040113923]
Robot Arm 1 togoal: 14.7920046419709
Mobile Platform 2 Pose: [17.8779855143415;-9.58881398987736;-8.92503522239276]
Robot Arm 2 end: [-9.71450923560823;-9.20234644438156;0.321678663612805]
Robot Arm 1 togoal: 1.46837591891767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.678238e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.716955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [-9.97803489722268;1.44386525289222;-1.76334555003429]
Robot Arm 1 end: [1.78522729224151;-2.16921069689432;0.776077326136594]
Robot Arm 1 togoal: 14.7012614906197
Mobile Platform 2 Pose: [18.2852881384031;-9.75723402117113;-8.97208811090096]
Robot Arm 2 end: [-9.9572617400954;-8.85303448966688;0.071631777005068]
Robot Arm 1 togoal: 1.84879006016932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.051225e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.139712e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [-9.93954471477539;1.42813186659369;-1.75767521830153]
Robot Arm 1 end: [1.80101972332983;-2.08912683774097;0.710204582785619]
Robot Arm 1 togoal: 14.6296840186243
Mobile Platform 2 Pose: [18.4059085572281;-9.78669428229631;-9.04232105387729]
Robot Arm 2 end: [-10.2863438003569;-8.66150780602308;0.331470357658248]
Robot Arm 1 togoal: 1.81343180057858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.691739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.961437e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-9.90434259220303;1.41542594832256;-1.7532736731327]
Robot Arm 1 end: [1.79400974634859;-2.02003165857045;0.656551202810419]
Robot Arm 1 togoal: 14.5796676518712
Mobile Platform 2 Pose: [18.5573928970302;-9.83818637620258;-9.12502326344556]
Robot Arm 2 end: [-10.3242876829902;-8.62297148884748;0.52359767857827]
Robot Arm 1 togoal: 1.73094812534736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.908408e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.940149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-9.87558279073225;1.4076201996387;-1.75040918308551]
Robot Arm 1 end: [1.76765691796197;-1.95710478527668;0.62011999267221]
Robot Arm 1 togoal: 14.5449445600205
Mobile Platform 2 Pose: [18.7576057503407;-9.9453080446095;-9.1344056059992]
Robot Arm 2 end: [-10.347192501024;-8.56752489591401;0.619596133797261]
Robot Arm 1 togoal: 1.72773160143545

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.267379e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.875149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-9.86198759589313;1.40858923031178;-1.74973764311197]
Robot Arm 1 end: [1.74110224108591;-1.88380600452693;0.601676758251107]
Robot Arm 1 togoal: 14.5010135014159
Mobile Platform 2 Pose: [18.8241765249037;-10.0078536793199;-9.01408792440023]
Robot Arm 2 end: [-10.2606132770126;-8.5243587217956;0.814027527492533]
Robot Arm 1 togoal: 1.65687855287565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.709443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.110952e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [-9.86604557860704;1.41830231607164;-1.7518316513523]
Robot Arm 1 end: [1.7928092851366;-1.78386922701364;0.588150544218618]
Robot Arm 1 togoal: 14.3905441695806
Mobile Platform 2 Pose: [18.8946202348373;-10.0570143958792;-9.01394417679061]
Robot Arm 2 end: [-10.1943768597054;-8.53048863000065;0.877983378990248]
Robot Arm 1 togoal: 1.6157711487216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.770614e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.329931e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [-9.85293206952108;1.42081759307741;-1.75246996759684]
Robot Arm 1 end: [1.83109045527777;-1.70537910345063;0.542149751181424]
Robot Arm 1 togoal: 14.3075200826336
Mobile Platform 2 Pose: [18.786474894921;-10.0090640768925;-9.0411465397531]
Robot Arm 2 end: [-10.0453591970816;-8.58920080184371;0.929998705253284]
Robot Arm 1 togoal: 1.53025957427836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.122352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.349440e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [-9.82482384679329;1.41643793555998;-1.74934831793085]
Robot Arm 1 end: [1.86375536566758;-1.65076422495582;0.491490915172348]
Robot Arm 1 togoal: 14.2477602706866
Mobile Platform 2 Pose: [18.6004983990505;-9.89119545459754;-9.05777908630942]
Robot Arm 2 end: [-9.88711587621204;-8.87621820047849;1.07646125483837]
Robot Arm 1 togoal: 1.21377224152188

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.765620e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.837780e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [-9.83398496850323;1.43272421895439;-1.74766254803057]
Robot Arm 1 end: [1.91577717859081;-1.61495666204601;0.422576820665653]
Robot Arm 1 togoal: 14.1939568256459
Mobile Platform 2 Pose: [18.835843984533;-9.95995506609353;-9.08537275568919]
Robot Arm 2 end: [-9.94823948762375;-8.92603117096398;1.16061240568427]
Robot Arm 1 togoal: 1.13400503284559

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.207732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.316949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [-9.85817665459898;1.45620795394911;-1.74109513152957]
Robot Arm 1 end: [2.00617946123738;-1.61109916624985;0.381306500026737]
Robot Arm 1 togoal: 14.1427611140227
Mobile Platform 2 Pose: [19.4331996295205;-10.1654172024446;-9.10570810118309]
Robot Arm 2 end: [-10.0342192934783;-9.38399387714782;0.888612650542079]
Robot Arm 1 togoal: 0.883486425018483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.592485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.132089e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [-9.91214443713225;1.50079171859005;-1.72933527320293]
Robot Arm 1 end: [2.1003339767054;-1.6579791711586;0.294925635584799]
Robot Arm 1 togoal: 14.135645014708
Mobile Platform 2 Pose: [19.5594008894784;-9.91282481981755;-9.10421095780842]
Robot Arm 2 end: [-9.8855896280623;-9.30730444988589;0.92554826821286]
Robot Arm 1 togoal: 0.920586564774394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.163465e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.030901e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [-10.0242363681066;1.5809521965392;-1.71620129666444]
Robot Arm 1 end: [2.2096319028465;-1.73354201198014;0.227456540888959]
Robot Arm 1 togoal: 14.1435178762803
Mobile Platform 2 Pose: [20.0160512682933;-10.0556241277892;-8.91611578243041]
Robot Arm 2 end: [-10.2603826747189;-9.28454702106645;-0.0311126604921909]
Robot Arm 1 togoal: 1.72879316669716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000835e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.035919e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [-10.188909636741;1.68932233621137;-1.70608924439448]
Robot Arm 1 end: [2.36947456302423;-1.87696535139691;0.315477712311452]
Robot Arm 1 togoal: 14.1682923596576
Mobile Platform 2 Pose: [19.4917122365925;-9.77824989915978;-8.83413668900456]
Robot Arm 2 end: [-9.95255480526964;-9.24838535842814;0.248784212223491]
Robot Arm 1 togoal: 1.478414227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000555e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991428e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [-10.3486683204908;1.79896362157276;-1.70110387311243]
Robot Arm 1 end: [2.51474337311367;-1.9456392036225;0.401963198364922]
Robot Arm 1 togoal: 14.1414145726968
Mobile Platform 2 Pose: [19.0238619168623;-9.55369605724155;-8.77884116074001]
Robot Arm 2 end: [-9.74583151546483;-9.09772241883686;0.521404888514985]
Robot Arm 1 togoal: 1.37036376152657

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.598070e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [-10.5261889450261;1.93691680271337;-1.69205486892013]
Robot Arm 1 end: [2.67255705823436;-2.01699331291401;0.516174320509875]
Robot Arm 1 togoal: 14.1106067549774
Mobile Platform 2 Pose: [18.3325553046703;-9.32476031605033;-8.63658973875398]
Robot Arm 2 end: [-9.37540706422269;-8.70682436028151;0.657154727937384]
Robot Arm 1 togoal: 1.67590221213804

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.052803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [-10.5908393483818;2.0989114261398;-1.64932257001565]
Robot Arm 1 end: [2.80445023487505;-2.01108247480707;0.400292123899416]
Robot Arm 1 togoal: 14.0462815286666
Mobile Platform 2 Pose: [17.8036868730518;-10.7884495083045;-7.26201972128381]
Robot Arm 2 end: [-10.5410418818478;-7.38928438712826;0.419903560519688]
Robot Arm 1 togoal: 2.88461014666487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956229e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.249636e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [-10.8116096419427;2.25991446893347;-1.64466068821875]
Robot Arm 1 end: [2.94943194990709;-2.12704526141295;0.531818611109842]
Robot Arm 1 togoal: 14.0625110283148
Mobile Platform 2 Pose: [17.6402628990161;-10.5195446959208;-7.38019803530159]
Robot Arm 2 end: [-10.603355034123;-7.32061186724048;0.722221805523404]
Robot Arm 1 togoal: 2.86028052245675

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.899773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.979926e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [-11.0819143574725;2.50258444034387;-1.62932342376465]
Robot Arm 1 end: [3.13263005890912;-2.22015245476204;0.678663335802102]
Robot Arm 1 togoal: 14.0428781587422
Mobile Platform 2 Pose: [16.6872036185602;-9.75709727155601;-7.66611581943838]
Robot Arm 2 end: [-9.90632572091648;-7.54471642167742;0.617238535110824]
Robot Arm 1 togoal: 2.61801014244807

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937427e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.837485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [-11.2871823362781;2.63667270116844;-1.63327360356543]
Robot Arm 1 end: [3.2091477378271;-2.25188695010205;0.824728157712776]
Robot Arm 1 togoal: 14.0253057992723
Mobile Platform 2 Pose: [15.3011530802787;-8.99666070460878;-7.6268031038285]
Robot Arm 2 end: [-9.11862853236455;-7.48903892133259;0.450217880188862]
Robot Arm 1 togoal: 2.86850406146683

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.212652e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [-11.4172372431657;2.70608285722048;-1.6396556419415]
Robot Arm 1 end: [3.19852209300726;-2.23547375258502;0.980781728733876]
Robot Arm 1 togoal: 14.0092382180516
Mobile Platform 2 Pose: [14.5278669134823;-8.58579497174001;-7.50523556327417]
Robot Arm 2 end: [-8.70581901010278;-7.40708726876945;0.519617653783902]
Robot Arm 1 togoal: 3.0660834087973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990361e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.659892e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [-11.583177822993;2.79492924504154;-1.64797902840831]
Robot Arm 1 end: [3.18336416100611;-2.17918952040895;1.14671320567166]
Robot Arm 1 togoal: 13.9620654682573
Mobile Platform 2 Pose: [14.7608285190633;-8.77887062274494;-7.3450825740547]
Robot Arm 2 end: [-9.10594498418015;-7.63839650792193;0.653946760142627]
Robot Arm 1 togoal: 2.66988515592996

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.931053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.914387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [-11.802031694684;2.91555991599514;-1.65806874624728]
Robot Arm 1 end: [3.24672618345949;-2.06258253648879;1.26930923185134]
Robot Arm 1 togoal: 13.826639249418
Mobile Platform 2 Pose: [14.3847241566038;-8.49465321974655;-7.34028607168554]
Robot Arm 2 end: [-8.24949483851701;-7.6184701856353;0.961906910431355]
Robot Arm 1 togoal: 3.00807873232898

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.992532e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.915494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [-12.0142710544414;3.03491305631673;-1.66733158784528]
Robot Arm 1 end: [3.35893637364791;-1.90495018024796;1.33737914254539]
Robot Arm 1 togoal: 13.6332417825965
Mobile Platform 2 Pose: [14.8259598748665;-8.66876305665745;-7.34096476384103]
Robot Arm 2 end: [-8.43333503551993;-7.58694918248073;1.12497468658547]
Robot Arm 1 togoal: 2.90415037622449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [-12.2310364008421;3.1629330755752;-1.67498086737776]
Robot Arm 1 end: [3.4727459715724;-1.73269524387854;1.3701566041894]
Robot Arm 1 togoal: 13.4269853864303
Mobile Platform 2 Pose: [15.5033195363379;-8.89689451109505;-7.36781035296819]
Robot Arm 2 end: [-8.60504746000036;-7.07448018683549;1.29774511135424]
Robot Arm 1 togoal: 3.24875383976284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.386095e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [-12.5810489863879;3.41201388128024;-1.67356196263072]
Robot Arm 1 end: [3.64837631159575;-1.6039241634802;1.37460819340391]
Robot Arm 1 togoal: 13.2293465459159
Mobile Platform 2 Pose: [15.3482836011198;-8.41931237232723;-7.41432113353364]
Robot Arm 2 end: [-8.43810629529606;-6.93212004096085;1.1202932006987]
Robot Arm 1 togoal: 3.46582823101119

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965571e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [-12.5489189124781;3.40187858270271;-1.66954677618439]
Robot Arm 1 end: [3.6282155269603;-1.68921832476515;1.39420898622358]
Robot Arm 1 togoal: 13.3136598415338
Mobile Platform 2 Pose: [14.8876629015769;-8.16965494125464;-7.42740582924146]
Robot Arm 2 end: [-8.24376741207925;-6.88122654737986;1.1125340132835]
Robot Arm 1 togoal: 3.60249152864337

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.724490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [-12.4680796802443;3.36979184297582;-1.66203420387217]
Robot Arm 1 end: [3.6425836453814;-1.87546767365782;1.3700222135078]
Robot Arm 1 togoal: 13.4709416696116
Mobile Platform 2 Pose: [14.4439877853563;-7.92645988350183;-7.42335108295768]
Robot Arm 2 end: [-8.13617947229701;-6.90593949078862;1.00605405625286]
Robot Arm 1 togoal: 3.64858968906207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000014e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.608274e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [-12.3293007552113;3.34566100771427;-1.6499990221057]
Robot Arm 1 end: [3.59772497665932;-2.0609890543866;1.31502234562918]
Robot Arm 1 togoal: 13.6564640093483
Mobile Platform 2 Pose: [14.1511874518833;-7.77245005697108;-7.43731498049023]
Robot Arm 2 end: [-8.21591747952978;-7.05527805886343;0.753403024883663]
Robot Arm 1 togoal: 3.52754062579955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.579916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [-12.161608180685;3.48977919225947;-1.6372556749706]
Robot Arm 1 end: [3.69084082038603;-2.21049282604048;1.23331999997209]
Robot Arm 1 togoal: 13.7471591389243
Mobile Platform 2 Pose: [13.6985562075472;-7.52711293832097;-7.46383021113138]
Robot Arm 2 end: [-7.87651212501266;-7.21310916652273;0.548444010920439]
Robot Arm 1 togoal: 3.63618294710709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.592863e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [-12.2869956462825;3.55240720813983;-1.64587765464858]
Robot Arm 1 end: [3.66976887405456;-2.1932563901525;1.26403184543816]
Robot Arm 1 togoal: 13.7409373820108
Mobile Platform 2 Pose: [13.3508772279281;-7.33502334076694;-7.48803352237906]
Robot Arm 2 end: [-7.50552543004588;-7.38621821879878;0.554707857176794]
Robot Arm 1 togoal: 3.74005067949206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.046279e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [-12.4358711594682;3.62866623282807;-1.65735676330249]
Robot Arm 1 end: [3.7069429641531;-2.18154839269373;1.27546872908489]
Robot Arm 1 togoal: 13.7132408169972
Mobile Platform 2 Pose: [13.0756196086787;-7.17278983604558;-7.58391804582485]
Robot Arm 2 end: [-7.29653642637489;-7.51742268873858;0.63274183120804]
Robot Arm 1 togoal: 3.77636173780181

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.062680e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [-12.5971077869604;3.7124978753131;-1.67012544435412]
Robot Arm 1 end: [3.78237669715398;-2.1802362793785;1.27511321128028]
Robot Arm 1 togoal: 13.6776260977718
Mobile Platform 2 Pose: [12.9356464726912;-7.07635242587994;-7.82491362940911]
Robot Arm 2 end: [-7.21364571185632;-7.81423552048849;0.745986530208812]
Robot Arm 1 togoal: 3.62518723065563

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015499e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [-12.7604454840097;3.79896048745327;-1.6830222862476]
Robot Arm 1 end: [3.87966015816404;-2.19249896511893;1.26307874807339]
Robot Arm 1 togoal: 13.6448566927684
Mobile Platform 2 Pose: [13.0000276795811;-7.07505165921959;-7.8607311208867]
Robot Arm 2 end: [-7.23285225885569;-7.75918690673741;0.990875174798814]
Robot Arm 1 togoal: 3.59991420322092

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.051811e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [-12.9204002633751;3.8854590124975;-1.69546095382941]
Robot Arm 1 end: [3.98635775595286;-2.22265749172112;1.23490544130065]
Robot Arm 1 togoal: 13.6249440107412
Mobile Platform 2 Pose: [13.0047027585398;-7.02308179932067;-7.86338953952716]
Robot Arm 2 end: [-6.97339752486983;-7.5514236675001;1.21950916883135]
Robot Arm 1 togoal: 3.9047080966215

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.038594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [-13.0774275208594;3.97250042426214;-1.70749956351689]
Robot Arm 1 end: [4.09453539515965;-2.27310938680167;1.18429898311126]
Robot Arm 1 togoal: 13.6241364421454
Mobile Platform 2 Pose: [12.9927049043601;-6.87171064275152;-7.80820964032161]
Robot Arm 2 end: [-6.69135719174541;-7.53525052379975;1.22646121839214]
Robot Arm 1 togoal: 4.13628581059327

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989890e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.605577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [-13.2363279810144;4.06368858781242;-1.71985534427189]
Robot Arm 1 end: [4.12258976363957;-2.39082162211727;1.05223605787925]
Robot Arm 1 togoal: 13.722104473842
Mobile Platform 2 Pose: [13.1536555790206;-6.70702509262176;-7.78018093796713]
Robot Arm 2 end: [-6.4258799527099;-7.40510810780451;1.13809699364503]
Robot Arm 1 togoal: 4.43332975954852

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.665549e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [-13.4209368289858;4.18053622738026;-1.7405903316418]
Robot Arm 1 end: [4.14739955352163;-2.47050396880403;0.918062339575855]
Robot Arm 1 togoal: 13.7887611859961
Mobile Platform 2 Pose: [13.0778909056067;-6.40286294672676;-7.74569962375393]
Robot Arm 2 end: [-6.23148535588223;-7.30349828799896;1.00592819845331]
Robot Arm 1 togoal: 4.66241601173718

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998144e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.684330e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [-13.6047418369049;4.33894618708363;-1.78399499938044]
Robot Arm 1 end: [4.17866709433953;-2.52091585262148;0.790680706298726]
Robot Arm 1 togoal: 13.8273141592178
Mobile Platform 2 Pose: [13.1236020509764;-6.20761016773893;-7.71751886353006]
Robot Arm 2 end: [-5.97177059455569;-7.1899482174379;0.9289087861856]
Robot Arm 1 togoal: 4.94707945853337

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000471e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.421348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [-13.5328657155683;4.39649704929456;-1.82394865701418]
Robot Arm 1 end: [4.10450730827185;-2.45194508367906;0.926378888284524]
Robot Arm 1 togoal: 13.7899000979585
Mobile Platform 2 Pose: [12.7078095959307;-5.96026996607092;-7.74251012572085]
Robot Arm 2 end: [-6.03326659761762;-7.2386166961117;1.07983776588613]
Robot Arm 1 togoal: 4.85333243796234

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001359e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.056132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [-13.7778933286057;4.65827464233984;-1.89716753401504]
Robot Arm 1 end: [4.26911582211535;-2.49415010480977;0.830200460817437]
Robot Arm 1 togoal: 13.7631400597822
Mobile Platform 2 Pose: [12.3589423801576;-5.66762633998593;-7.70146668995715]
Robot Arm 2 end: [-6.00193967335014;-7.38250655055608;1.22188932601695]
Robot Arm 1 togoal: 4.78802939931493

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000095e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.020074e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [-13.6944841695678;4.76157347857736;-1.90224794518369]
Robot Arm 1 end: [4.33114879847276;-2.41263689392918;0.927785240811497]
Robot Arm 1 togoal: 13.6587456362621
Mobile Platform 2 Pose: [12.7996636759005;-5.81088082263722;-7.52011472567365]
Robot Arm 2 end: [-6.19213300533348;-7.10908447183062;1.15744084298576]
Robot Arm 1 togoal: 4.79472824055028

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.226051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009936e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [-14.1297710626703;5.19047917467407;-1.8870294053995]
Robot Arm 1 end: [4.6655459059578;-2.31178173650987;0.889147334694669]
Robot Arm 1 togoal: 13.4326321992212
Mobile Platform 2 Pose: [12.9327608974765;-5.86658442504935;-7.47970686340376]
Robot Arm 2 end: [-6.2425375052658;-6.93184221661531;1.05088449541293]
Robot Arm 1 togoal: 4.87371779760907

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.858913e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [-14.5415739120149;5.45440777106359;-1.81027960561198]
Robot Arm 1 end: [4.9181414039218;-2.21630288808254;0.824291407171415]
Robot Arm 1 togoal: 13.2494809674654
Mobile Platform 2 Pose: [12.7496622827341;-5.7702289148172;-7.50183042467217]
Robot Arm 2 end: [-6.34659662962729;-6.99660883279999;0.8928883390065]
Robot Arm 1 togoal: 4.77097882499407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.515267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.896434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [-14.5789575167165;5.47536848888865;-1.79186259039174]
Robot Arm 1 end: [4.92644317424456;-2.10971605403191;0.960346209130291]
Robot Arm 1 togoal: 13.1415575350361
Mobile Platform 2 Pose: [12.1258978439155;-5.43328258072316;-7.54588953799764]
Robot Arm 2 end: [-6.16674731135036;-7.16850913690707;0.737563780427225]
Robot Arm 1 togoal: 4.82958994045633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000262e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.864142e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [-14.6119857801083;5.50186750560253;-1.77708408399515]
Robot Arm 1 end: [4.96631381681598;-1.98982181177436;1.09669589827436]
Robot Arm 1 togoal: 13.0105287225239
Mobile Platform 2 Pose: [11.0883680760748;-4.84872352179963;-7.64659546052987]
Robot Arm 2 end: [-5.66632103722753;-7.69164239872025;0.371409674666656]
Robot Arm 1 togoal: 5.04290056255948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.780979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [-14.6803674317641;5.54752036428611;-1.76084241667593]
Robot Arm 1 end: [4.97584133487388;-1.86088320983315;1.13545582143892]
Robot Arm 1 togoal: 12.8868679330273
Mobile Platform 2 Pose: [10.9711286885772;-4.80324197574735;-7.64753790342934]
Robot Arm 2 end: [-5.60480742861581;-7.79068316440714;0.331115327164124]
Robot Arm 1 togoal: 5.06108922606928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998788e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.751998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [-14.7559752993014;5.58955078468181;-1.74458982641272]
Robot Arm 1 end: [4.96758755140013;-1.71281131339884;1.11817467864669]
Robot Arm 1 togoal: 12.7545047789254
Mobile Platform 2 Pose: [10.7891181127381;-4.7411715322109;-7.64479273059441]
Robot Arm 2 end: [-5.56041058738284;-7.71802103404221;0.360866103530833]
Robot Arm 1 togoal: 5.12477246435711

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999567e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.571673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [-14.8294450757691;5.62539623307384;-1.72911970873762]
Robot Arm 1 end: [4.96122132820101;-1.57335944466529;1.06115141064427]
Robot Arm 1 togoal: 12.6310490484335
Mobile Platform 2 Pose: [10.6961589499353;-4.71980410869301;-7.63755904716272]
Robot Arm 2 end: [-5.54008379013557;-7.53180935160061;0.454845547436652]
Robot Arm 1 togoal: 5.2076389076604

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999933e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.505328e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [-14.8933639092878;5.65136585514691;-1.71152972866522]
Robot Arm 1 end: [4.94411137547007;-1.50057426300993;0.986463466686855]
Robot Arm 1 togoal: 12.5742175688233
Mobile Platform 2 Pose: [10.5821423368608;-4.67632341627366;-7.62661611930575]
Robot Arm 2 end: [-5.45968567715412;-7.39250833081949;0.554847605534761]
Robot Arm 1 togoal: 5.32418232261219

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.987841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.528197e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [-14.9268543011639;5.66657854445023;-1.70750017625051]
Robot Arm 1 end: [4.94436207567753;-1.48695350062028;0.982037679063283]
Robot Arm 1 togoal: 12.5618492241648
Mobile Platform 2 Pose: [10.474705868756;-4.63378800138448;-7.61510583973153]
Robot Arm 2 end: [-5.36504991048174;-7.29417556944624;0.695723281043268]
Robot Arm 1 togoal: 5.4300395803492

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.819317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.648735e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [-14.9831546228138;5.70182988233751;-1.71131197180543]
Robot Arm 1 end: [4.94318980668843;-1.53495718825662;0.937456194424694]
Robot Arm 1 togoal: 12.6082151805526
Mobile Platform 2 Pose: [10.3811961121639;-4.5960405480264;-7.60551432124642]
Robot Arm 2 end: [-5.25626049755115;-7.2075591163907;0.868066523222094]
Robot Arm 1 togoal: 5.54320418897262

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.756779e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.710049e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [-15.0787031115144;5.76112782823574;-1.71421221596777]
Robot Arm 1 end: [4.97371725328188;-1.62212308828161;0.865984583533318]
Robot Arm 1 togoal: 12.6793654622356
Mobile Platform 2 Pose: [10.2614280791352;-4.54106901221169;-7.59640947527767]
Robot Arm 2 end: [-5.13111554901039;-7.16628249183919;1.01901206477894]
Robot Arm 1 togoal: 5.65579195164152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.670783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.714998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [-15.1993102956207;5.83427108890274;-1.71424839078112]
Robot Arm 1 end: [5.04762444786112;-1.74040097445482;0.792069002923647]
Robot Arm 1 togoal: 12.763008228819
Mobile Platform 2 Pose: [10.0676308586258;-4.44022151380675;-7.58938163453279]
Robot Arm 2 end: [-4.93207554264813;-7.11092462590724;1.10218463932278]
Robot Arm 1 togoal: 5.84859137989257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.523276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.910798e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [-15.3304581513349;5.91352148542078;-1.71166266952157]
Robot Arm 1 end: [5.16516606383375;-1.85644260403443;0.739577059712159]
Robot Arm 1 togoal: 12.8281515513606
Mobile Platform 2 Pose: [10.0906624030516;-4.43525960967833;-7.62452395389421]
Robot Arm 2 end: [-4.95678050092995;-7.27605603856643;1.16946618028017]
Robot Arm 1 togoal: 5.74260477895159

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.339301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942428e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [-15.4584874000752;5.99197403606359;-1.70690200391004]
Robot Arm 1 end: [5.30290601550035;-1.93177352295593;0.718762729615513]
Robot Arm 1 togoal: 12.848093085759
Mobile Platform 2 Pose: [10.0722397148557;-4.4232628594532;-7.63203801050529]
Robot Arm 2 end: [-5.03087384279553;-7.53711612113442;1.13586345494739]
Robot Arm 1 togoal: 5.55934725712693

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.141054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.158558e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [-15.57829433988;6.06649441778954;-1.70081478040277]
Robot Arm 1 end: [5.43196868320998;-1.97217272579326;0.721320080843054]
Robot Arm 1 togoal: 12.8389765114087
Mobile Platform 2 Pose: [9.93365979445978;-4.31723198355653;-7.66860830844931]
Robot Arm 2 end: [-4.8166792651985;-7.75277245253623;1.26179123184278]
Robot Arm 1 togoal: 5.65544292477545

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.633923e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.508652e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [-15.6952352230472;6.13730717124494;-1.69315407220968]
Robot Arm 1 end: [5.55996972104065;-1.99359498161015;0.740188015991138]
Robot Arm 1 togoal: 12.8128785530556
Mobile Platform 2 Pose: [9.87306573093695;-4.19521852783919;-7.69584302513344]
Robot Arm 2 end: [-4.60253101129346;-8.00818394255012;1.31234036354041]
Robot Arm 1 togoal: 5.75704277686482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.987096e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.046940e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [-15.8065504811101;6.20288604720885;-1.6845159652093]
Robot Arm 1 end: [5.68720772604829;-1.99311931188108;0.774478068124286]
Robot Arm 1 togoal: 12.7668470274396
Mobile Platform 2 Pose: [10.3396736990287;-4.39832437881417;-7.71701110788701]
Robot Arm 2 end: [-4.86385171009809;-7.93779600880029;1.29369857758103]
Robot Arm 1 togoal: 5.5393474790643

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.408714e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.045488e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [-15.9131002097509;6.26376892885778;-1.67585771424161]
Robot Arm 1 end: [5.80516847061823;-1.96566226040697;0.829235349442794]
Robot Arm 1 togoal: 12.6985126381708
Mobile Platform 2 Pose: [11.051702503241;-4.53589554562632;-7.7150501028033]
Robot Arm 2 end: [-4.97589383622603;-7.72573317111042;1.31056962634615]
Robot Arm 1 togoal: 5.51889602148973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.923791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [-16.0145299580936;6.31915278117656;-1.66748152158002]
Robot Arm 1 end: [5.8900093524413;-1.92261228722309;0.877377820926319]
Robot Arm 1 togoal: 12.6275475205544
Mobile Platform 2 Pose: [10.4967956226726;-4.07136077891937;-7.61744165161765]
Robot Arm 2 end: [-4.39929231214056;-7.60684314587415;1.39003656752638]
Robot Arm 1 togoal: 6.09198471386318

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003498e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.048170e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [-16.127310210258;6.37814562776039;-1.65928116928107]
Robot Arm 1 end: [5.98715497018925;-1.85962633358458;0.890247444741721]
Robot Arm 1 togoal: 12.5360085669839
Mobile Platform 2 Pose: [11.2356728070528;-4.34024289386777;-7.6559732518396]
Robot Arm 2 end: [-4.66101513771395;-7.34010387648795;1.27173986792637]
Robot Arm 1 togoal: 5.97008688052656

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983274e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.830971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [-16.2401342812341;6.43656080414274;-1.65172611143439]
Robot Arm 1 end: [6.08198758762981;-1.79922889939364;0.890679058808568]
Robot Arm 1 togoal: 12.4486918337903
Mobile Platform 2 Pose: [11.5967793649008;-4.52211051766587;-7.71536901977096]
Robot Arm 2 end: [-4.95920698097373;-7.31708857005124;1.15252570065256]
Robot Arm 1 togoal: 5.72218326435752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999960e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.618889e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [-16.3518996799309;6.49374099495333;-1.64450478220155]
Robot Arm 1 end: [6.16201004332576;-1.73909247127507;0.926021849945893]
Robot Arm 1 togoal: 12.3648881093086
Mobile Platform 2 Pose: [11.660847774012;-4.55521028711363;-7.72129211071318]
Robot Arm 2 end: [-5.03776255699512;-7.29596146592554;1.07493392730138]
Robot Arm 1 togoal: 5.66873881703258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.626392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [-16.4539001389389;6.54643257279501;-1.63819781382403]
Robot Arm 1 end: [6.24203206594274;-1.68843911070905;0.975791606038856]
Robot Arm 1 togoal: 12.2897999996339
Mobile Platform 2 Pose: [11.6024284318035;-4.51914347316382;-7.71851026879169]
Robot Arm 2 end: [-5.01546003276571;-7.27055623275214;1.01448746930663]
Robot Arm 1 togoal: 5.70544099148117

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.636359e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.467163e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [-16.5449959172983;6.59423490915093;-1.6329409603914]
Robot Arm 1 end: [6.32756421484947;-1.59111012824701;1.0175282293132]
Robot Arm 1 togoal: 12.1693920318594
Mobile Platform 2 Pose: [11.4879954890927;-4.45031656698873;-7.71614413993957]
Robot Arm 2 end: [-5.02791422930673;-7.36903282861299;1.02537008003019]
Robot Arm 1 togoal: 5.64705889694945

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.927994e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.680830e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [-16.6156951821351;6.63250371362025;-1.63021027428329]
Robot Arm 1 end: [6.41286689227604;-1.59922904874535;1.05972198574749]
Robot Arm 1 togoal: 12.1499965376217
Mobile Platform 2 Pose: [11.4954963711487;-4.44462762643015;-7.72163537234613]
Robot Arm 2 end: [-5.11785497753414;-7.50368299247157;0.855269234175081]
Robot Arm 1 togoal: 5.52359810947807

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.727604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [-16.6880259089968;6.67181434865147;-1.62720075905028]
Robot Arm 1 end: [6.50735991395538;-1.60142970613684;1.10835808086434]
Robot Arm 1 togoal: 12.1227876064257
Mobile Platform 2 Pose: [11.538538611699;-4.4743399722637;-7.71998785170364]
Robot Arm 2 end: [-5.21539725386676;-7.69727015067939;0.723131211781246]
Robot Arm 1 togoal: 5.36950489342096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.693373e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [-16.7621977388665;6.71210752364062;-1.62345894180822]
Robot Arm 1 end: [6.60883703657876;-1.59209030443279;1.16268959358162]
Robot Arm 1 togoal: 12.0832499857428
Mobile Platform 2 Pose: [11.6215265391791;-4.53903957854398;-7.71764347802742]
Robot Arm 2 end: [-5.29494793995878;-7.71005892491916;0.606021987810363]
Robot Arm 1 togoal: 5.3121116119516

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.703660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [-16.8385057922852;6.75322089343176;-1.61835300040318]
Robot Arm 1 end: [6.71256240765173;-1.56778298284503;1.22151882569289]
Robot Arm 1 togoal: 12.0295693204515
Mobile Platform 2 Pose: [11.6940517848473;-4.62434546157497;-7.69366668280477]
Robot Arm 2 end: [-5.37482066749198;-7.75415124863924;0.534850319486187]
Robot Arm 1 togoal: 5.23532345363928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973822e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [-16.9153455757103;6.79400116983489;-1.61139369648236]
Robot Arm 1 end: [6.79053342916645;-1.52290425438224;1.26509980810755]
Robot Arm 1 togoal: 11.9642585659609
Mobile Platform 2 Pose: [11.7888351749539;-4.80456903892148;-7.57667508099327]
Robot Arm 2 end: [-5.52633569116913;-7.75344659126061;0.54096464377305]
Robot Arm 1 togoal: 5.10109241887271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990346e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [-16.9893994808131;6.83149138129141;-1.6028626255502]
Robot Arm 1 end: [6.86158751081811;-1.4724650471167;1.29677807412805]
Robot Arm 1 togoal: 11.896107038927
Mobile Platform 2 Pose: [11.6870835567673;-4.90452201508662;-7.38717627231709]
Robot Arm 2 end: [-5.46571515154788;-7.75045460748408;0.353743924622152]
Robot Arm 1 togoal: 5.19448556696412

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000987e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.34228787469168;9.99051056545028;9.8185121340509]
Robot Arm 2 end: [9.76840314945189;9.82900815021592;1.48915542089247]
Robot Arm 1 togoal: 27.9996489509745

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.14395648901189;9.85630566141316;9.81438031167463]
Robot Arm 2 end: [9.54404910616651;9.88298897242625;1.46052084831193]
Robot Arm 1 togoal: 27.8802217292055

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-1.98909032909345;9.74117417657346;9.82682867297555]
Robot Arm 2 end: [9.24609926237172;9.99412733289485;1.34444452110038]
Robot Arm 1 togoal: 27.7526329644271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.865495e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.88279618361794;9.6488993109932;9.85009589733347]
Robot Arm 2 end: [8.99387070016169;9.97820350256194;1.20350282136791]
Robot Arm 1 togoal: 27.5680348197451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.916287e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.73500238027792;9.53302276425293;9.86232306624327]
Robot Arm 2 end: [8.82130456710542;10.04302597723;1.12378795427988]
Robot Arm 1 togoal: 27.4976757807078

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.838255e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.59219187294753;9.46335895477355;9.85530066862422]
Robot Arm 2 end: [8.70857325500942;9.97005145148899;1.07167563248226]
Robot Arm 1 togoal: 27.3681486513139

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.882483e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.50309535894015;9.4133772944559;9.8503924705836]
Robot Arm 2 end: [8.66689856311648;9.80843810707144;1.09009663240626]
Robot Arm 1 togoal: 27.2215539585741

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.925289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.4336954102145;9.37254691446114;9.84257431236426]
Robot Arm 2 end: [8.68030385956486;9.67945833897754;1.12713680125014]
Robot Arm 1 togoal: 27.1365060577178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.36686144085779;9.33404719191078;9.83130882250871]
Robot Arm 2 end: [8.67815483283561;9.58757544671199;1.11109371415913]
Robot Arm 1 togoal: 27.0687015389211

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.30567298518505;9.29882658226068;9.81658540437275]
Robot Arm 2 end: [8.64061437804722;9.48768101910587;1.03934319377083]
Robot Arm 1 togoal: 26.9717298142419

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.24604462915409;9.26700731668562;9.80409466626252]
Robot Arm 2 end: [8.64340683445106;9.34301934679763;0.966539686037292]
Robot Arm 1 togoal: 26.8707357935209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.15408071559582;9.2212276567848;9.78996780245469]
Robot Arm 2 end: [8.68575268211836;9.19151595512636;0.889655929070189]
Robot Arm 1 togoal: 26.7931004792103

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.00550287737645;9.14618488805256;9.76576062050878]
Robot Arm 2 end: [8.74855252358908;9.0518370127019;0.807472203797654]
Robot Arm 1 togoal: 26.7392938800614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.762260249364818;9.01208977479977;9.70328380149486]
Robot Arm 2 end: [8.78083156177774;8.92525867457385;0.703150759004594]
Robot Arm 1 togoal: 26.674968180532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.660639114854416;8.87663099700436;9.52998190415118]
Robot Arm 2 end: [8.74268812020602;8.71891839260159;0.718147485967451]
Robot Arm 1 togoal: 26.5015251621388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.919654440080286;8.96955801284379;9.46295060821859]
Robot Arm 2 end: [8.71737620286756;8.68679313391488;0.870713152744879]
Robot Arm 1 togoal: 26.45674361166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998062e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-1.02322325488985;8.97152967872514;9.27643268752244]
Robot Arm 2 end: [8.71502981180434;8.5164134584375;0.939886994908286]
Robot Arm 1 togoal: 26.3333932592292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996893e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.10713811109957;8.99329030607152;9.11966398818476]
Robot Arm 2 end: [8.63849306904434;8.37310350918155;0.814398589006549]
Robot Arm 1 togoal: 26.1813605833985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.25203252778248;9.04151175426865;8.95026065389541]
Robot Arm 2 end: [8.81908031978163;8.18891840226777;0.886399315950927]
Robot Arm 1 togoal: 26.1800927186086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.38749693020338;9.10840532979233;8.81177894257671]
Robot Arm 2 end: [8.94863624578469;8.05301732309237;0.895351976606708]
Robot Arm 1 togoal: 26.1792605986516

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.37766268067037;9.11394830173448;8.64633730514882]
Robot Arm 2 end: [8.96456310682906;7.86904564524088;0.859692663433347]
Robot Arm 1 togoal: 26.0652023490707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.001516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.47971995676139;9.1706488664306;8.49963025386304]
Robot Arm 2 end: [9.09152282228589;7.72657504413148;0.853988068551444]
Robot Arm 1 togoal: 26.0607484809202
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.39528387203092;9.14638545716867;8.34562148985282]
Robot Arm 2 end: [8.9999748203422;7.54194189723855;0.73151316222176]
Robot Arm 1 togoal: 25.8716458341319
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.59047828274038;9.24138832873689;8.22715406435792]
Robot Arm 2 end: [9.26065591568803;7.47588536933577;0.93135105723163]
Robot Arm 1 togoal: 26.0139793499649
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.4069010899239;9.1726395011578;8.10478457813489]
Robot Arm 2 end: [9.02900462284155;7.30046580437753;0.730263532673071]
Robot Arm 1 togoal: 25.7300291127958
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.59810608852534;9.26186417828825;7.97651841373933]
Robot Arm 2 end: [9.28263905020603;7.21454828457883;0.901978193806872]
Robot Arm 1 togoal: 25.8562183889905

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.327170e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-1.40519636380428;9.18404078270378;7.85022521239588]
Robot Arm 2 end: [9.04385753411785;7.04230088898455;0.697532475102269]
Robot Arm 1 togoal: 25.569251614684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-1.59917848035069;9.27101351462127;7.72041399405685]
Robot Arm 2 end: [9.29499594099644;6.93563538669514;0.822243366462743]
Robot Arm 1 togoal: 25.6826960256719

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.933859e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-1.41734520643394;9.19434709427858;7.58060692452161]
Robot Arm 2 end: [9.05628263844319;6.7707246975398;0.612971521818025]
Robot Arm 1 togoal: 25.4012525398668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997794e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-1.63154951541919;9.28700489168819;7.45505885747225]
Robot Arm 2 end: [9.31341603826541;6.65791338989432;0.756316299471528]
Robot Arm 1 togoal: 25.5162469562865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948127e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-1.43613091256602;9.19821710336586;7.3136151659285]
Robot Arm 2 end: [9.06516389653477;6.50812465139562;0.544177984584604]
Robot Arm 1 togoal: 25.2379245414029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-1.64804057959987;9.28510743456744;7.17946448281814]
Robot Arm 2 end: [9.31543333458283;6.37402193105995;0.636323240246299]
Robot Arm 1 togoal: 25.3372692307359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006132e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-1.47544572166419;9.20285584421319;7.02801405150881]
Robot Arm 2 end: [9.06674314798289;6.24587528487956;0.426846834548117]
Robot Arm 1 togoal: 25.0732193623492

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.847649e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-1.72987719490236;9.30860905224942;6.91707992375697]
Robot Arm 2 end: [9.28550542766403;6.13471899270936;0.448805851065897]
Robot Arm 1 togoal: 25.1676275573834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.811330e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-1.83229531679328;9.33866879627002;6.72523432211775]
Robot Arm 2 end: [9.22141841060513;5.95875527412373;0.430512399459366]
Robot Arm 1 togoal: 25.0066782844474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.794941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-1.97133813171762;9.35934636043599;6.53594017527161]
Robot Arm 2 end: [9.13249019537772;5.83871146347041;0.354799908389154]
Robot Arm 1 togoal: 24.8651761416035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.793025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-2.01289765375194;9.28881510868911;6.3222303752833]
Robot Arm 2 end: [9.00138506393994;5.63464715662754;0.36415861453452]
Robot Arm 1 togoal: 24.6339827890556

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-2.09797978838599;9.21595324699901;6.12409756961765]
Robot Arm 2 end: [8.84765048166203;5.52886331328499;0.299030852427832]
Robot Arm 1 togoal: 24.451436217106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-2.02244673362443;9.03254955811186;5.93567342229256]
Robot Arm 2 end: [8.69427522715678;5.35753873081741;0.211078227926751]
Robot Arm 1 togoal: 24.2290283995499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-2.22069548362675;8.973935693697;5.78782152945973]
Robot Arm 2 end: [8.6429470815167;5.15684573619025;0.322221068089898]
Robot Arm 1 togoal: 24.0567354286173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.818358e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-2.16089468173469;8.78754878332727;5.62862972371633]
Robot Arm 2 end: [8.35758443227873;5.04388499235537;0.329477196534801]
Robot Arm 1 togoal: 23.7642401191924

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.075280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [-2.22238186245156;8.67882946732654;5.52972554200734]
Robot Arm 2 end: [8.19319327478945;5.05324485256797;0.260634567985204]
Robot Arm 1 togoal: 23.6470079110958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.530842e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-2.29558360506573;8.49782411256793;5.42404523253747]
Robot Arm 2 end: [7.98115636723415;5.04585058534616;0.223383055122651]
Robot Arm 1 togoal: 23.481554758472

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-2.07285330254739;8.17617752049812;5.34592030924663]
Robot Arm 2 end: [7.81418591205616;4.9658308982095;0.0767873238490922]
Robot Arm 1 togoal: 23.3110931626352

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.686680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-2.23434724897528;8.01661965041678;5.28979990093533]
Robot Arm 2 end: [7.53574751142573;4.87163709822772;0.242142552744838]
Robot Arm 1 togoal: 23.0283196767862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.940557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-1.99541936737261;7.81542209742967;5.25521802025521]
Robot Arm 2 end: [7.39440314204678;4.79337733296236;0.187146292307258]
Robot Arm 1 togoal: 22.8733128185879

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.519298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307231655121;-6.24962959891394;-7.10439671995214]
Robot Arm 1 end: [-6.1385981594231;-6.93244679791835;1.24046701735592]
Robot Arm 1 togoal: 23.3931785756024
Mobile Platform 2 Pose: [-1.63827637840857;7.55360899565582;5.19556511935404]
Robot Arm 2 end: [7.13408906769115;4.88074040338176;0.0433854803612159]
Robot Arm 1 togoal: 22.7419609488949

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.713055e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407570605985;-6.21793453087098;-7.10337750236049]
Robot Arm 1 end: [-6.10318947839368;-6.88092488745415;1.17440543315355]
Robot Arm 1 togoal: 23.3323480221491
Mobile Platform 2 Pose: [-1.27134347806746;7.19710086161799;5.12307234242302]
Robot Arm 2 end: [6.76792933600729;4.90692888194788;-0.0277371711395659]
Robot Arm 1 togoal: 22.4895213318126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.998248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.210103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859364857263;-6.18978135858564;-7.10219131493518]
Robot Arm 1 end: [-6.07853623671703;-6.82861065222798;1.10280524177264]
Robot Arm 1 togoal: 23.2786672912242
Mobile Platform 2 Pose: [-1.38464700978483;7.00669928433607;5.0677268023323]
Robot Arm 2 end: [6.56546671208992;4.71679407223885;0.0176819549515179]
Robot Arm 1 togoal: 22.2094277346043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000075e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70846081650858;-6.16355464009722;-7.1010798166555]
Robot Arm 1 end: [-6.05862409561346;-6.77632343185466;1.02306544203945]
Robot Arm 1 togoal: 23.2286972204303
Mobile Platform 2 Pose: [-1.08016252844789;6.8023872725717;5.03783244599789]
Robot Arm 2 end: [6.27614969592821;4.77023306149861;0.0379319881301061]
Robot Arm 1 togoal: 22.0288974836028

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000172e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.745752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74537921859019;-6.13815627724101;-7.10000571910372]
Robot Arm 1 end: [-6.04163959809546;-6.72117944871986;0.931004874525134]
Robot Arm 1 togoal: 23.1793040749622
Mobile Platform 2 Pose: [-0.832430505667866;6.47011442772401;4.96426396410852]
Robot Arm 2 end: [5.85707363732118;4.79671819426912;0.0697411048437319]
Robot Arm 1 togoal: 21.7369686488436

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.017437e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78014784531988;-6.11348972608299;-7.09883287552191]
Robot Arm 1 end: [-6.02900518375034;-6.65908558807977;0.819195285560995]
Robot Arm 1 togoal: 23.1289141480717
Mobile Platform 2 Pose: [-1.13300764166219;6.47598369906124;4.94125008129282]
Robot Arm 2 end: [5.84359593456897;4.61485206982954;0.194066096928265]
Robot Arm 1 togoal: 21.5956983388825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976870e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80660557751526;-6.09324645078319;-7.09791056536176]
Robot Arm 1 end: [-5.9968630341706;-6.60126474576941;0.729518200153498]
Robot Arm 1 togoal: 23.0679011080902
Mobile Platform 2 Pose: [-1.24509267461092;6.31399026274617;4.86562009125194]
Robot Arm 2 end: [5.702001423816;4.40926404791971;0.273193283248254]
Robot Arm 1 togoal: 21.3479919598674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.795597e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82836063377096;-6.07554654758533;-7.09843674979444]
Robot Arm 1 end: [-5.97846018076126;-6.56323891196918;0.64670472544231]
Robot Arm 1 togoal: 23.0307721370834
Mobile Platform 2 Pose: [-1.00707397801593;6.07312268350575;4.79866630406656]
Robot Arm 2 end: [5.43643609184323;4.45392505016103;0.188185276090297]
Robot Arm 1 togoal: 21.1890515045271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.682543e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85402160063901;-6.05624385479217;-7.10074738439077]
Robot Arm 1 end: [-5.94941183153267;-6.53392169451335;0.572727604568698]
Robot Arm 1 togoal: 22.9924666990749
Mobile Platform 2 Pose: [-0.715725815100598;5.80238213572822;4.73957027113118]
Robot Arm 2 end: [5.14773059575191;4.52752030455411;0.132839719956181]
Robot Arm 1 togoal: 21.0340100162523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998316e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89425845343058;-6.02968433065478;-7.10427062212004]
Robot Arm 1 end: [-5.9149583910258;-6.50993507910348;0.51188141696391]
Robot Arm 1 togoal: 22.9539142025695
Mobile Platform 2 Pose: [-0.390296211455;5.55490370368314;4.70414277508345]
Robot Arm 2 end: [4.9419821335886;4.67747690330011;0.0891293066540397]
Robot Arm 1 togoal: 20.9938422402821

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983968e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013493e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94445859415608;-5.99880491103356;-7.1084156484586]
Robot Arm 1 end: [-5.88617262235789;-6.48958705592352;0.463025628806503]
Robot Arm 1 togoal: 22.9215241995888
Mobile Platform 2 Pose: [-0.520891181117447;5.46308390982751;4.68656908134214]
Robot Arm 2 end: [4.82053581899988;4.71075974421092;0.141470589060107]
Robot Arm 1 togoal: 20.9274421659648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981821e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004876e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99662352316764;-5.96827013162707;-7.11344981607046]
Robot Arm 1 end: [-5.86049403902183;-6.4728754943218;0.425194801637972]
Robot Arm 1 togoal: 22.8934856945384
Mobile Platform 2 Pose: [-0.656126410508975;5.29209797524453;4.68258333954294]
Robot Arm 2 end: [4.72696324347292;4.57437990278577;0.0842354200885241]
Robot Arm 1 togoal: 20.7692149197657

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002893e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.04697402277021;-5.94065500216226;-7.11962020164088]
Robot Arm 1 end: [-5.81028239972321;-6.46904256615549;0.3892029461312]
Robot Arm 1 togoal: 22.8577198595132
Mobile Platform 2 Pose: [-0.422359428885089;4.9847053558636;4.64757973950954]
Robot Arm 2 end: [4.33435605678539;4.67179996899009;0.142708855189149]
Robot Arm 1 togoal: 20.5580924302967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.746469e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982718e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.07944573024934;-5.92447680162438;-7.12418663227825]
Robot Arm 1 end: [-5.76066806918128;-6.47577005322976;0.374319568311566]
Robot Arm 1 togoal: 22.8290282939557
Mobile Platform 2 Pose: [-0.703762811725033;4.93504620246586;4.64642704088559]
Robot Arm 2 end: [4.29146386717541;4.53770267415161;0.172516464503364]
Robot Arm 1 togoal: 20.4305933921245

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.826535e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007522e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.12038194093704;-5.89745132999441;-7.12349702725167]
Robot Arm 1 end: [-5.75910986936228;-6.48362792778596;0.380694656439075]
Robot Arm 1 togoal: 22.8333052756904
Mobile Platform 2 Pose: [-0.630754712881438;4.67343147446497;4.61062067518102]
Robot Arm 2 end: [4.05913044470341;4.68827075895795;0.113086884957551]
Robot Arm 1 togoal: 20.3810369237964

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.415621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007982e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.1666805856113;-5.86921259567456;-7.12403148138721]
Robot Arm 1 end: [-5.64085482946713;-6.55413731612984;0.3470489648136]
Robot Arm 1 togoal: 22.8046916907632
Mobile Platform 2 Pose: [-0.887438412857327;4.63061906516005;4.62305654756765]
Robot Arm 2 end: [3.98072143427877;4.55125774967349;0.182518848157469]
Robot Arm 1 togoal: 20.2235309902641

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.130364e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003923e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.19876803078832;-5.84901800633362;-7.12843980139412]
Robot Arm 1 end: [-5.65277528181517;-6.5610430549657;0.349095663917227]
Robot Arm 1 togoal: 22.8177755459283
Mobile Platform 2 Pose: [-0.844481377804997;4.38161312686509;4.60078393614668]
Robot Arm 2 end: [3.7565028484557;4.67481027490922;0.128264125850639]
Robot Arm 1 togoal: 20.1626174005086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.551026e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007970e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.22189365625092;-5.83406966922908;-7.13223156695533]
Robot Arm 1 end: [-5.67443212305061;-6.57042063571802;0.334376267442063]
Robot Arm 1 togoal: 22.8402000496366
Mobile Platform 2 Pose: [-1.05472418896464;4.30454765266284;4.62066601812225]
Robot Arm 2 end: [3.6565108850924;4.54910240135653;0.183039121851152]
Robot Arm 1 togoal: 19.9991702215683

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.600198e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007777e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.24310168098529;-5.82068485904432;-7.13545140999362]
Robot Arm 1 end: [-5.71745698512936;-6.58111917671042;0.315876928356133]
Robot Arm 1 togoal: 22.8784677994282
Mobile Platform 2 Pose: [-0.919715696682478;4.03741196538898;4.61088914002015]
Robot Arm 2 end: [3.46092751069858;4.74017176056916;0.0805989466546589]
Robot Arm 1 togoal: 20.0135950836885

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004638e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.26034258879196;-5.80975459442974;-7.13802317480267]
Robot Arm 1 end: [-5.7629024541153;-6.5949318700772;0.293764916889113]
Robot Arm 1 togoal: 22.9208848760181
Mobile Platform 2 Pose: [-1.17322089712396;4.01756109166484;4.64197693105153]
Robot Arm 2 end: [3.36361266723053;4.62502801396713;0.181746490253972]
Robot Arm 1 togoal: 19.8562631905516

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006612e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.27505223361132;-5.80037745686491;-7.14021083039509]
Robot Arm 1 end: [-5.81187672482673;-6.61187177099243;0.270382415300538]
Robot Arm 1 togoal: 22.968103396671
Mobile Platform 2 Pose: [-1.08914396746401;3.76762530229183;4.64764274964116]
Robot Arm 2 end: [3.14906129525927;4.77162848945417;0.126260018845377]
Robot Arm 1 togoal: 19.8253403701417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004828e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.28845757075611;-5.79177666867366;-7.14215998935864]
Robot Arm 1 end: [-5.8830109352107;-6.64295180979876;0.246742127941301]
Robot Arm 1 togoal: 23.0408683524691
Mobile Platform 2 Pose: [-1.28928641931315;3.71359824456957;4.6835592359464]
Robot Arm 2 end: [3.06066630488026;4.66343521565298;0.180446168577455]
Robot Arm 1 togoal: 19.6823377987989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002116e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.29978043971141;-5.78450434413264;-7.14365111563479]
Robot Arm 1 end: [-5.95035882179597;-6.68207519454551;0.222832275497917]
Robot Arm 1 togoal: 23.116894661296
Mobile Platform 2 Pose: [-1.13382021824058;3.46388722236135;4.69557612691939]
Robot Arm 2 end: [2.84559289501925;4.81719923906694;0.125368546407767]
Robot Arm 1 togoal: 19.6597669688384

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005868e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.31184474155037;-5.77674485440117;-7.14502749244184]
Robot Arm 1 end: [-6.01490020308893;-6.72814597218485;0.207587882358746]
Robot Arm 1 togoal: 23.1955372339678
Mobile Platform 2 Pose: [-1.30808067256534;3.39669751996661;4.73348488235378]
Robot Arm 2 end: [2.74400746736539;4.71238836295506;0.180146416628835]
Robot Arm 1 togoal: 19.5105608838577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007668e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.32533459433114;-5.76804907629652;-7.1463483702443]
Robot Arm 1 end: [-6.10651865142751;-6.783016019804;0.219167845320236]
Robot Arm 1 togoal: 23.2977324379163
Mobile Platform 2 Pose: [-1.13567436158791;3.14697683951773;4.74529935469659]
Robot Arm 2 end: [2.52872751761435;4.86570859355137;0.125020173610867]
Robot Arm 1 togoal: 19.4912048081872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970610e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006363e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.33779632950334;-5.76005183838395;-7.14737152799727]
Robot Arm 1 end: [-6.16889016926748;-6.7877744510212;0.256437528352219]
Robot Arm 1 togoal: 23.3422684966328
Mobile Platform 2 Pose: [-1.30522603964846;3.07714237756203;4.78260524310516]
Robot Arm 2 end: [2.42459643585626;4.76077363623879;0.179976581302374]
Robot Arm 1 togoal: 19.3403562207026

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981001e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003865e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.35412620619395;-5.74931105513306;-7.14880814045338]
Robot Arm 1 end: [-6.21558600915426;-6.78466945661054;0.319723906177792]
Robot Arm 1 togoal: 23.3690697855422
Mobile Platform 2 Pose: [-1.12308932281581;2.82739720875214;4.79389228900219]
Robot Arm 2 end: [2.2088536415338;4.91266291307514;0.124879876619716]
Robot Arm 1 togoal: 19.3233737685298

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978551e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002130e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.36563192917324;-5.74150374674096;-7.15001610407495]
Robot Arm 1 end: [-6.26204691750011;-6.79370177862366;0.375019201995852]
Robot Arm 1 togoal: 23.4050392301683
Mobile Platform 2 Pose: [-1.28725202643591;2.75396316693391;4.8305837250327]
Robot Arm 2 end: [2.10129896601732;4.80788433291185;0.180125423565555]
Robot Arm 1 togoal: 19.1706238738221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.37364308104302;-5.73567893906806;-7.15118518001386]
Robot Arm 1 end: [-6.25379258493005;-6.83946925147177;0.305512605717692]
Robot Arm 1 togoal: 23.4356759593328
Mobile Platform 2 Pose: [-1.0980633810739;2.504190338541;4.841238968232]
Robot Arm 2 end: [1.95268051225259;4.94993882743334;0.049377757801201]
Robot Arm 1 togoal: 19.1972111098387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996190e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001254e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.39770916396324;-5.72084290449465;-7.15314262728933]
Robot Arm 1 end: [-6.23168301226435;-6.88870015957751;0.254798211759802]
Robot Arm 1 togoal: 23.4584524949811
Mobile Platform 2 Pose: [-1.26774677084094;2.42484898167226;4.88422405844516]
Robot Arm 2 end: [1.78555850243672;4.79474081069111;0.168203680016819]
Robot Arm 1 togoal: 18.9634860390501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.714642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.40677122732997;-5.71395671063646;-7.15532309833415]
Robot Arm 1 end: [-6.21811807433367;-6.94097119917163;0.214708391938107]
Robot Arm 1 togoal: 23.4889390311586
Mobile Platform 2 Pose: [-1.08561859360299;2.19845393271348;4.88508204566252]
Robot Arm 2 end: [1.60993710287149;4.96889199337677;0.083928397021912]
Robot Arm 1 togoal: 18.9979878364551

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.886918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.41178741330445;-5.71009561673023;-7.15724144602013]
Robot Arm 1 end: [-6.21700818455663;-6.99718260734501;0.182805815990818]
Robot Arm 1 togoal: 23.5305404680709
Mobile Platform 2 Pose: [-1.1975198933585;2.13582923442014;4.91211957007918]
Robot Arm 2 end: [1.51046544238145;4.8299497430261;0.139503431119498]
Robot Arm 1 togoal: 18.8235691846888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.923673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.41677761811945;-5.70693309683261;-7.15873518231644]
Robot Arm 1 end: [-6.23323492804711;-7.04853273618159;0.173145381076035]
Robot Arm 1 togoal: 23.5793786338737
Mobile Platform 2 Pose: [-1.18389154825312;1.94724798832089;4.91935970193431]
Robot Arm 2 end: [1.33197999546452;4.77127504242879;0.135708718870535]
Robot Arm 1 togoal: 18.6687805956967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.746942e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.42011429525938;-5.70526218803673;-7.15971501004908]
Robot Arm 1 end: [-6.25446286340927;-7.08929949736026;0.177639950916655]
Robot Arm 1 togoal: 23.6232160492576
Mobile Platform 2 Pose: [-1.13566325291712;1.77052951221836;4.91548978436239]
Robot Arm 2 end: [1.15502638693648;4.81197259697194;0.132029279601836]
Robot Arm 1 togoal: 18.594579467383

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.41752302086313;-5.70767070692223;-7.15998345556561]
Robot Arm 1 end: [-6.28254163658245;-7.12083741892773;0.191173573362886]
Robot Arm 1 togoal: 23.664586926405
Mobile Platform 2 Pose: [-1.10921651100572;1.61682168675478;4.91919418875289]
Robot Arm 2 end: [0.986830752595135;4.70504654303127;0.157676440275212]
Robot Arm 1 togoal: 18.4067241775224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.204393e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.40443577542183;-5.71702617482353;-7.15906480045711]
Robot Arm 1 end: [-6.3303982129878;-7.13992005808327;0.222178319459233]
Robot Arm 1 togoal: 23.7096120413237
Mobile Platform 2 Pose: [-0.863690168455819;1.4225704603823;4.90750989445375]
Robot Arm 2 end: [0.802906488590356;4.89200824876484;0.0867110945100854]
Robot Arm 1 togoal: 18.453505979811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.827062e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.37598967461793;-5.73687623182012;-7.15547977268293]
Robot Arm 1 end: [-6.39736956688018;-7.1403948924894;0.266816270156147]
Robot Arm 1 togoal: 23.7537332352523
Mobile Platform 2 Pose: [-0.816682402765482;1.17292450757436;4.92081020450855]
Robot Arm 2 end: [0.525468978999736;4.95603445971973;0.109601508355645]
Robot Arm 1 togoal: 18.3428599103418

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997393e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.33554137227804;-5.76850635214135;-7.14368902998506]
Robot Arm 1 end: [-6.45525795548538;-7.1069308525443;0.280549774404521]
Robot Arm 1 togoal: 23.7689148750522
Mobile Platform 2 Pose: [-0.915411713741235;1.04210717237935;4.94549237017362]
Robot Arm 2 end: [0.330088726888397;4.86697295114921;0.182210983755317]
Robot Arm 1 togoal: 18.1529604711079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.27516924255959;-5.81909585396948;-7.11506238701306]
Robot Arm 1 end: [-6.51597640761614;-7.04928210814505;0.281877313340811]
Robot Arm 1 togoal: 23.7695376928673
Mobile Platform 2 Pose: [-0.718788429302267;0.812002421358012;4.94669948696463]
Robot Arm 2 end: [0.0825276049350003;4.97621383652482;0.198173087016996]
Robot Arm 1 togoal: 18.1023262268501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.19673674692638;-5.88866072279481;-7.06193505046019]
Robot Arm 1 end: [-6.59705201707208;-6.96132495570268;0.301968641272521]
Robot Arm 1 togoal: 23.7620436276828
Mobile Platform 2 Pose: [-0.759500336712685;0.669669997965044;4.96250065808014]
Robot Arm 2 end: [-0.106646697225063;4.86764155718146;0.281681678745411]
Robot Arm 1 togoal: 17.9014277348974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-4.16295671808535;-5.94060162514828;-6.98484138524264]
Robot Arm 1 end: [-6.65676820097517;-6.91410510963081;0.302834042596857]
Robot Arm 1 togoal: 23.7701242499999
Mobile Platform 2 Pose: [-0.636109443943126;0.467111090504735;4.95719086207091]
Robot Arm 2 end: [-0.31423792808809;4.98191558247537;0.296248875505084]
Robot Arm 1 togoal: 17.8821642076975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-4.07970535271872;-6.01545052971762;-6.91269050570053]
Robot Arm 1 end: [-6.74364815984228;-6.80236314508209;0.33808752874471]
Robot Arm 1 togoal: 23.7501251128169
Mobile Platform 2 Pose: [-0.657965098291524;0.321255328436185;4.96399786320527]
Robot Arm 2 end: [-0.493976825157736;4.87218912669819;0.383630782629275]
Robot Arm 1 togoal: 17.6872862462714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-4.07961699589682;-6.04575237833605;-6.82177257293006]
Robot Arm 1 end: [-6.77714017016869;-6.73604152267427;0.326210803919079]
Robot Arm 1 togoal: 23.7275165021803
Mobile Platform 2 Pose: [-0.574653601624995;0.130425416980773;4.95281063369735]
Robot Arm 2 end: [-0.694009452506305;4.99438212134732;0.437644596883711]
Robot Arm 1 togoal: 17.6806847774763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-4.08044287975781;-6.0748992607186;-6.72869246255646]
Robot Arm 1 end: [-6.80022172823337;-6.60474486745102;0.315221879035063]
Robot Arm 1 togoal: 23.6520380328618
Mobile Platform 2 Pose: [-0.656325905972985;0.0256182763411268;4.95918789277224]
Robot Arm 2 end: [-0.816564799735694;4.86483515545658;0.52758055235533]
Robot Arm 1 togoal: 17.5010196410183

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-4.18901731075878;-6.03593236782088;-6.63982653822048]
Robot Arm 1 end: [-6.73049764769914;-6.57176902395552;0.224687189209477]
Robot Arm 1 togoal: 23.5841791679596
Mobile Platform 2 Pose: [-0.56435572482518;-0.168036640748884;4.94851787814241]
Robot Arm 2 end: [-1.0094352640454;4.9889486691596;0.593825223476344]
Robot Arm 1 togoal: 17.5030994267962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-4.1865609399888;-6.0649942058489;-6.52838292178449]
Robot Arm 1 end: [-6.78484477265219;-6.43672902860849;0.270050445150726]
Robot Arm 1 togoal: 23.5257720381438
Mobile Platform 2 Pose: [-0.659846099163827;-0.266722752118044;4.95365313752631]
Robot Arm 2 end: [-1.10909344514759;4.85917708354254;0.68442801383553]
Robot Arm 1 togoal: 17.3361822687473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-4.35469001709574;-5.98511586487486;-6.44788939833882]
Robot Arm 1 end: [-6.66961676236295;-6.37323979743525;0.195455746805124]
Robot Arm 1 togoal: 23.4033367713997
Mobile Platform 2 Pose: [-0.556436103036848;-0.469963998838404;4.94251199577397]
Robot Arm 2 end: [-1.29592555579862;4.97865408823739;0.758442947420641]
Robot Arm 1 togoal: 17.3407751696315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-4.53511854752659;-5.89690631748816;-6.35389542572151]
Robot Arm 1 end: [-6.54895781857004;-6.23929963678844;0.154058225569075]
Robot Arm 1 togoal: 23.2260928136631
Mobile Platform 2 Pose: [-0.673627893242749;-0.562229819418338;4.94632854098405]
Robot Arm 2 end: [-1.37354183679404;4.85598063289917;0.847386461549358]
Robot Arm 1 togoal: 17.1921405358903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-4.75222422496891;-5.78168412325717;-6.28795702823042]
Robot Arm 1 end: [-6.32770384716688;-6.19816387946582;0.0363442141254329]
Robot Arm 1 togoal: 23.0473128322881
Mobile Platform 2 Pose: [-0.552380441857338;-0.782102233177841;4.93477218755144]
Robot Arm 2 end: [-1.55758659811559;4.96297554492462;0.922134841324089]
Robot Arm 1 togoal: 17.1908004666626

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-4.94092236107915;-5.68651988641827;-6.14852866616429]
Robot Arm 1 end: [-6.27603284646583;-6.03532510643096;0.0891847171878218]
Robot Arm 1 togoal: 22.8930336891436
Mobile Platform 2 Pose: [-0.708300292786518;-0.857872619802344;4.93747122692443]
Robot Arm 2 end: [-1.60545361125977;4.85764974685861;1.00746308852281]
Robot Arm 1 togoal: 17.0728405730414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.767049e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-5.12784306467442;-5.58572841516004;-6.10391590489604]
Robot Arm 1 end: [-6.15423153702264;-5.91040514823157;0.0980030304872683]
Robot Arm 1 togoal: 22.7183870225827
Mobile Platform 2 Pose: [-0.542965497385472;-1.10378237975825;4.92620833432897]
Robot Arm 2 end: [-1.78179414785869;5.05619290366506;1.08161873155409]
Robot Arm 1 togoal: 17.1586977384534

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.061190e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-5.32614858489583;-5.48551932236093;-6.07859628124697]
Robot Arm 1 end: [-6.0698722458363;-5.72211585668876;0.229124287872895]
Robot Arm 1 togoal: 22.5187624851862
Mobile Platform 2 Pose: [-0.780167686790353;-1.09741231454916;4.94214531961298]
Robot Arm 2 end: [-1.74762596283019;4.98690467900993;1.15480918389009]
Robot Arm 1 togoal: 17.1126586134618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003803e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.191486e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-5.4300188900813;-5.43911513193733;-6.06519963437001]
Robot Arm 1 end: [-5.95865987444333;-5.58912417807325;0.29145040288947]
Robot Arm 1 togoal: 22.3430394046851
Mobile Platform 2 Pose: [-0.555946343885629;-1.3882027906343;4.94146762086526]
Robot Arm 2 end: [-1.97330763396375;5.28065975069684;1.10305205490653]
Robot Arm 1 togoal: 17.2656026028737

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.030939e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.236272e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-5.51875797722684;-5.42013693344644;-6.03291946337991]
Robot Arm 1 end: [-5.83943130494874;-5.49398380298797;0.275819752192671]
Robot Arm 1 togoal: 22.1923768893955
Mobile Platform 2 Pose: [-0.819028075032326;-1.31986422426638;4.96828902085802]
Robot Arm 2 end: [-1.90062926243978;5.21050711404685;1.16126184560205]
Robot Arm 1 togoal: 17.2362624878669

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.049951e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-5.45812753953378;-5.47817170040469;-6.00600445343393]
Robot Arm 1 end: [-5.9099437509502;-5.60991273967273;0.123608012565873]
Robot Arm 1 togoal: 22.3326753942545
Mobile Platform 2 Pose: [-0.78894199026122;-1.56542922350111;5.01517005292117]
Robot Arm 2 end: [-2.12267221450582;5.33637855392011;1.11261757172973]
Robot Arm 1 togoal: 17.2459727587915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.340821e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.575859e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-5.38701749814906;-5.56942391708136;-5.94261837554123]
Robot Arm 1 end: [-5.99192109649897;-5.70494434505099;0.0159881140085303]
Robot Arm 1 togoal: 22.4644581064128
Mobile Platform 2 Pose: [-0.997572058731396;-1.51722243487511;5.04184278731658]
Robot Arm 2 end: [-2.11618870140767;5.12438379355059;1.17851427176122]
Robot Arm 1 togoal: 17.0592720302033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.880061e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-5.35730097318269;-5.66544263578997;-5.83427314234715]
Robot Arm 1 end: [-6.14477590466144;-5.62469148833747;0.0454178887631756]
Robot Arm 1 togoal: 22.5158192319192
Mobile Platform 2 Pose: [-0.996788831316447;-1.73320042764361;5.07741425797476]
Robot Arm 2 end: [-2.26903787604118;5.15815156705077;1.21569337223645]
Robot Arm 1 togoal: 17.0185353784844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.011246e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-5.21322115948436;-5.81417285177191;-5.76547499969175]
Robot Arm 1 end: [-6.30063880604721;-5.71764760972148;0.00254938353504769]
Robot Arm 1 togoal: 22.6949545966331
Mobile Platform 2 Pose: [-1.19554860211657;-1.74134823250028;5.11188407562971]
Robot Arm 2 end: [-2.31580344990424;5.02744386533021;1.20416187640767]
Robot Arm 1 togoal: 16.8810939261268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.718289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.362561e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-5.12423502219111;-5.87943304423143;-5.75526053575975]
Robot Arm 1 end: [-6.42716810148656;-5.73797212063308;0.0359504684950737]
Robot Arm 1 togoal: 22.7978286337832
Mobile Platform 2 Pose: [-0.962376973404685;-1.97815744649705;5.11771032675221]
Robot Arm 2 end: [-2.37548667801972;5.2287111729337;1.31339011842785]
Robot Arm 1 togoal: 17.0320271620444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.144900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.733081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-4.90545676682898;-5.9821439596558;-5.77688031126861]
Robot Arm 1 end: [-6.5328380129555;-5.83609546918875;0.0197064150472049]
Robot Arm 1 togoal: 22.942766504938
Mobile Platform 2 Pose: [-1.26525327961022;-2.01647366656122;5.17084616227462]
Robot Arm 2 end: [-2.35347753475777;5.07393140876327;1.36013981903223]
Robot Arm 1 togoal: 16.9032124143719

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-4.73356135805105;-6.03322161697652;-5.8221235787152]
Robot Arm 1 end: [-6.65187060774534;-5.82258653579198;0.0712161317066872]
Robot Arm 1 togoal: 23.0161011616755
Mobile Platform 2 Pose: [-1.46901112418849;-2.16589841893262;5.21400903125321]
Robot Arm 2 end: [-2.53258603809242;5.01547102565957;1.30405745367652]
Robot Arm 1 togoal: 16.771216576901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.647761e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-4.63204167906937;-6.07317094778035;-5.84649258409813]
Robot Arm 1 end: [-6.73563358197635;-5.80301646228974;0.129336650421255]
Robot Arm 1 togoal: 23.0597807356474
Mobile Platform 2 Pose: [-1.13145434294133;-2.43471234321223;5.19596254973297]
Robot Arm 2 end: [-2.76535203210426;5.12380771850165;1.34150953115314]
Robot Arm 1 togoal: 16.7660939981259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.121125e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-4.55320747194594;-6.11052356079569;-5.86113331912453]
Robot Arm 1 end: [-6.81066671839627;-5.75481085206061;0.205289957141836]
Robot Arm 1 togoal: 23.0769077958482
Mobile Platform 2 Pose: [-1.38518299088802;-2.48285467937135;5.21516616310399]
Robot Arm 2 end: [-2.83780959720263;5.07868707496309;1.30476033122622]
Robot Arm 1 togoal: 16.694625927673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.521795e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-4.50043920544021;-6.13887191328184;-5.86766652807533]
Robot Arm 1 end: [-6.86526843343346;-5.67673984893293;0.296122517579812]
Robot Arm 1 togoal: 23.0585727327133
Mobile Platform 2 Pose: [-1.19251285586015;-2.61371453726601;5.20667083907901]
Robot Arm 2 end: [-2.92312875871513;5.14644201299365;1.35399189000137]
Robot Arm 1 togoal: 16.7189923119639

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.879178e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-4.46482187348521;-6.15950255435434;-5.8702309409364]
Robot Arm 1 end: [-6.88198575935232;-5.67354294474661;0.297284384602549]
Robot Arm 1 togoal: 23.0685689093875
Mobile Platform 2 Pose: [-1.32203833547652;-2.86364754405609;5.20088345044885]
Robot Arm 2 end: [-2.96190667635974;5.1623831435298;1.3977610548142]
Robot Arm 1 togoal: 16.7166924976527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.320664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-4.42905006167268;-6.18086027010131;-5.87205569717086]
Robot Arm 1 end: [-6.90133310822249;-5.67412314104659;0.297056156796234]
Robot Arm 1 togoal: 23.0831374732099
Mobile Platform 2 Pose: [-1.19795165172372;-2.95414425316917;5.18063028661069]
Robot Arm 2 end: [-3.05404934801443;5.14861501768903;0.837002439051965]
Robot Arm 1 togoal: 16.6791672477182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.809664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-4.39209548056552;-6.20341984515025;-5.87328646356489]
Robot Arm 1 end: [-6.92373063320201;-5.67758423193997;0.296087868961405]
Robot Arm 1 togoal: 23.1019418149972
Mobile Platform 2 Pose: [-1.15229360736361;-2.99188283869617;5.16779744218832]
Robot Arm 2 end: [-3.14940620840617;5.08424852558043;0.880686949448415]
Robot Arm 1 togoal: 16.5793603520041

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995047e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.955941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-4.35363729990551;-6.22725438293871;-5.87404652427622]
Robot Arm 1 end: [-6.94897819035806;-5.6830651298553;0.294662383066792]
Robot Arm 1 togoal: 23.1242361498758
Mobile Platform 2 Pose: [-1.11734929757534;-3.0197781715794;5.15976057905204]
Robot Arm 2 end: [-3.24401972040982;5.04332726287761;0.899633582911757]
Robot Arm 1 togoal: 16.5024562081083

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984391e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-4.31398210059068;-6.25203025947249;-5.87448070576308]
Robot Arm 1 end: [-6.97689764121227;-5.69138827280459;0.292983352229044]
Robot Arm 1 togoal: 23.1504372385342
Mobile Platform 2 Pose: [-1.09403730410493;-3.04408961597278;5.15138331686563]
Robot Arm 2 end: [-3.3355383563199;5.01818167917406;0.90842122244915]
Robot Arm 1 togoal: 16.4419002158348

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979073e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-4.28362133914724;-6.27219072786468;-5.87481968094442]
Robot Arm 1 end: [-6.99130387112973;-5.66222204255885;0.305910228982822]
Robot Arm 1 togoal: 23.140571507703
Mobile Platform 2 Pose: [-1.06481142925641;-3.07890609832404;5.14136727093736]
Robot Arm 2 end: [-3.43290579587531;5.01208207547693;0.922117847422191]
Robot Arm 1 togoal: 16.3965848382942

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973761e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-4.30666322288631;-6.26835471381156;-5.87281162770123]
Robot Arm 1 end: [-6.95851365119206;-5.56250425434247;0.348104102992025]
Robot Arm 1 togoal: 23.0468741635241
Mobile Platform 2 Pose: [-1.01302041070387;-3.14251469323139;5.12842376720464]
Robot Arm 2 end: [-3.54832579808819;5.0279877204726;0.951941542553644]
Robot Arm 1 togoal: 16.3642397459302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.926380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-4.29491683468013;-6.29294598872458;-5.85103416214223]
Robot Arm 1 end: [-6.97209691007867;-5.59243346195037;0.293321104129217]
Robot Arm 1 togoal: 23.0799317646699
Mobile Platform 2 Pose: [-0.957541275492698;-3.24872549056829;5.11159433663366]
Robot Arm 2 end: [-3.68495235903043;5.06804585198029;1.00143403549959]
Robot Arm 1 togoal: 16.3461243509452

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984779e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-4.29110153891148;-6.3163101751944;-5.82212189805902]
Robot Arm 1 end: [-6.99469632731314;-5.53921919510485;0.31795764178884]
Robot Arm 1 togoal: 23.0593657190903
Mobile Platform 2 Pose: [-0.847650386197218;-3.40965800714808;5.10054229913099]
Robot Arm 2 end: [-3.87449523604421;5.12426797296034;0.969634044970885]
Robot Arm 1 togoal: 16.3269499523801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-4.31513576900387;-6.32300479561854;-5.79434809129989]
Robot Arm 1 end: [-6.98161024650834;-5.52704113498529;0.288627004450638]
Robot Arm 1 togoal: 23.0430648172504
Mobile Platform 2 Pose: [-0.790379558181235;-3.48893718744663;5.09545672650079]
Robot Arm 2 end: [-4.00063217875575;5.2122775773736;0.961866035035629]
Robot Arm 1 togoal: 16.3621035975336

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-4.35424545594946;-6.31391937146305;-5.77099403490771]
Robot Arm 1 end: [-7.00090313333722;-5.50814169682;0.348131318300096]
Robot Arm 1 togoal: 23.0414580090383
Mobile Platform 2 Pose: [-0.734644605268625;-3.53541651703459;5.09311742277881]
Robot Arm 2 end: [-4.11219509858877;5.27264762817461;0.92237676382975]
Robot Arm 1 togoal: 16.3792051672342

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992395e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-4.35121980867913;-6.33040686232451;-5.74783769243769]
Robot Arm 1 end: [-7.02132848856933;-5.44167377253052;0.404020941000045]
Robot Arm 1 togoal: 23.009097214608
Mobile Platform 2 Pose: [-0.676002591328068;-3.57664824764013;5.09106720358808]
Robot Arm 2 end: [-4.22873582306113;5.27801788534119;0.863888094323116]
Robot Arm 1 togoal: 16.3449416261241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919619e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-4.37754041992031;-6.3306651762249;-5.72320562668414]
Robot Arm 1 end: [-6.99040219651601;-5.44711611611539;0.315344842604641]
Robot Arm 1 togoal: 22.9943638194815
Mobile Platform 2 Pose: [-0.605138854276639;-3.6233314557028;5.0884438819441]
Robot Arm 2 end: [-4.34969009733387;5.2342410719425;0.793418244703566]
Robot Arm 1 togoal: 16.2646081491337

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-4.41830874419576;-6.31560040933808;-5.70673476347916]
Robot Arm 1 end: [-7.00585244057296;-5.42301831338873;0.396076895709231]
Robot Arm 1 togoal: 22.985516376976
Mobile Platform 2 Pose: [-0.51046872815558;-3.68460914267699;5.08440529477714]
Robot Arm 2 end: [-4.46870851432885;5.15971078654759;0.713286864867706]
Robot Arm 1 togoal: 16.1574879588348

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000716e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-4.43132744850435;-6.32013303420291;-5.68459595665118]
Robot Arm 1 end: [-7.01664865028593;-5.3585680858668;0.474243923661096]
Robot Arm 1 togoal: 22.9466259658983
Mobile Platform 2 Pose: [-0.382417616909038;-3.76679449942473;5.07771270875106]
Robot Arm 2 end: [-4.58212356166583;5.05757267970324;0.630278371556819]
Robot Arm 1 togoal: 16.0273910891629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.920967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005628e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-4.44429222001148;-6.32571795563774;-5.66085284178228]
Robot Arm 1 end: [-7.02235034508885;-5.37177447259796;0.428220410626359]
Robot Arm 1 togoal: 22.9618385356729
Mobile Platform 2 Pose: [-0.213738661060049;-3.87227993785725;5.06689224562067]
Robot Arm 2 end: [-4.68354248474358;4.93614754614349;0.548757800438856]
Robot Arm 1 togoal: 15.8839062867514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.873648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013291e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-4.46958731855208;-6.32414267839567;-5.63840963043798]
Robot Arm 1 end: [-7.00283884203707;-5.41810340819928;0.310233734908981]
Robot Arm 1 togoal: 22.9843511185957
Mobile Platform 2 Pose: [-0.0105123688349285;-3.99684666629888;5.05222847346378]
Robot Arm 2 end: [-4.76465646529796;4.81036948909984;0.469504528415668]
Robot Arm 1 togoal: 15.7436180421391

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942573e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.961688e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-4.47686473653927;-6.33146478045078;-5.6240633476012]
Robot Arm 1 end: [-6.97180622691992;-5.36145200028926;0.274807778940105]
Robot Arm 1 togoal: 22.9253006351746
Mobile Platform 2 Pose: [0.226302761391921;-4.1455290478042;5.03330516877366]
Robot Arm 2 end: [-4.83295131558597;4.69704170856762;0.392195932349671]
Robot Arm 1 togoal: 15.6197191304333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.953355e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.692040e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-4.52140142151419;-6.31115798214551;-5.61454282664045]
Robot Arm 1 end: [-6.975752948351;-5.39231902547928;0.288305721746348]
Robot Arm 1 togoal: 22.9481852949033
Mobile Platform 2 Pose: [0.507479564940984;-4.34015833502926;5.00782074205277]
Robot Arm 2 end: [-4.9315430766361;4.56856027726781;0.383275736888692]
Robot Arm 1 togoal: 15.4669525322622

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.950975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-4.54726131582284;-6.30700875504742;-5.59898412650454]
Robot Arm 1 end: [-6.94134502962179;-5.33611850100138;0.269706844258979]
Robot Arm 1 togoal: 22.8860533218607
Mobile Platform 2 Pose: [0.734542696071044;-4.58810689419781;4.97585964059012]
Robot Arm 2 end: [-5.07184749649152;4.43879863951802;0.396024051039305]
Robot Arm 1 togoal: 15.2980771385205

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.932851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-4.58414024587599;-6.28953404550876;-5.59262279406604]
Robot Arm 1 end: [-6.93695611873402;-5.34945205825531;0.277542831777178]
Robot Arm 1 togoal: 22.8913159688517
Mobile Platform 2 Pose: [0.581537217750586;-4.6548930006629;4.96941696153432]
Robot Arm 2 end: [-5.22285625850261;4.56768975761234;0.278274307183083]
Robot Arm 1 togoal: 15.3812565919875

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-4.62386532376051;-6.27226664683005;-5.58301019869134]
Robot Arm 1 end: [-6.94195579304811;-5.37948432689773;0.292243428560194]
Robot Arm 1 togoal: 22.914367697293
Mobile Platform 2 Pose: [0.847076279895114;-4.9014264579989;4.92792895828063]
Robot Arm 2 end: [-5.3558469524565;4.39969044226696;0.328968480492454]
Robot Arm 1 togoal: 15.1769621962243

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980742e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-4.62403336941868;-6.27942307385026;-5.57246243115889]
Robot Arm 1 end: [-6.96970737506463;-5.30588230713284;0.374480548758879]
Robot Arm 1 togoal: 22.8813353729016
Mobile Platform 2 Pose: [0.768089127068267;-5.01023157294742;4.90534327620541]
Robot Arm 2 end: [-5.57361566270835;4.50392027681002;0.270896526745654]
Robot Arm 1 togoal: 15.215759606123

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-4.62863799921606;-6.28981769307521;-5.54772113306924]
Robot Arm 1 end: [-6.98631879264411;-5.22367655424025;0.458607214332229]
Robot Arm 1 togoal: 22.8347111298676
Mobile Platform 2 Pose: [1.04278024855189;-5.20911815656007;4.87307824044454]
Robot Arm 2 end: [-5.66478170131652;4.34112861432687;0.337203458195981]
Robot Arm 1 togoal: 15.0287545020302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-4.61809660156618;-6.31098564744292;-5.51385363704884]
Robot Arm 1 end: [-6.99163314037281;-5.11807693693361;0.5387279295101]
Robot Arm 1 togoal: 22.7648128801562
Mobile Platform 2 Pose: [1.08401147273448;-5.38478891567804;4.83966203987684]
Robot Arm 2 end: [-5.82486715353873;4.321027146096;0.271954472588213]
Robot Arm 1 togoal: 14.9694244222804

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-4.58184552915073;-6.34550219157029;-5.47480288676887]
Robot Arm 1 end: [-6.9759059612657;-4.982254953042;0.610631174547265]
Robot Arm 1 togoal: 22.6600555622012
Mobile Platform 2 Pose: [1.3476724537293;-5.6269241580924;4.77744850124493]
Robot Arm 2 end: [-5.9405963592681;4.1461429982755;0.348030550467594]
Robot Arm 1 togoal: 14.7637385839625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-4.54356536089023;-6.37916457782562;-5.42353145789645]
Robot Arm 1 end: [-6.94328459803639;-4.84612324995664;0.686625611936758]
Robot Arm 1 togoal: 22.5428136917825
Mobile Platform 2 Pose: [1.16899997681249;-5.64336675524386;4.74369686130787]
Robot Arm 2 end: [-6.09184190112057;4.21301847908954;0.276837219037394]
Robot Arm 1 togoal: 14.7929555883302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','4.247454e-10','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-4.48453047424739;-6.42021598256328;-5.36441015460016]
Robot Arm 1 end: [-6.87794443691213;-4.69515091801884;0.754478084670196]
Robot Arm 1 togoal: 22.3919633968423
Mobile Platform 2 Pose: [1.4705082959089;-5.84697455053659;4.69146515305379]
Robot Arm 2 end: [-6.18012908152191;4.04124252520752;0.422537191841102]
Robot Arm 1 togoal: 14.592961523503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.756052e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.645293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-4.4448267751843;-6.44462304662094;-5.28752875335993]
Robot Arm 1 end: [-6.73162026647106;-4.5186417780379;0.645744184691732]
Robot Arm 1 togoal: 22.1698928405756
Mobile Platform 2 Pose: [1.8255176156988;-6.08841216784395;4.62659700255572]
Robot Arm 2 end: [-6.28109407233505;3.89304332063232;0.546082125896429]
Robot Arm 1 togoal: 14.4151787664612

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.428703e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-4.52607698987505;-6.3989543892969;-5.27356508054046]
Robot Arm 1 end: [-6.57051118993462;-4.48901311147669;0.782129179125622]
Robot Arm 1 togoal: 22.0240612088287
Mobile Platform 2 Pose: [2.22577886554094;-6.30544036623537;4.56226000009788]
Robot Arm 2 end: [-6.45974417081204;3.8073467430374;0.772583263509992]
Robot Arm 1 togoal: 14.2736247334033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.062939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.642859e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-4.62344855974325;-6.34160021978597;-5.25662158098539]
Robot Arm 1 end: [-6.36123981080719;-4.49647927922163;0.914448005448803]
Robot Arm 1 togoal: 21.8679213681076
Mobile Platform 2 Pose: [2.69397045471388;-6.52926149459158;4.47492395376483]
Robot Arm 2 end: [-6.84993013594542;3.79224599251525;0.912418926799302]
Robot Arm 1 togoal: 14.1604858881726

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.825930e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-4.73890918436938;-6.26638672321992;-5.23170541735294]
Robot Arm 1 end: [-6.36630941349235;-4.56132746873937;1.08093400459888]
Robot Arm 1 togoal: 21.9107735954334
Mobile Platform 2 Pose: [3.16956553095671;-6.75358489160371;4.38158370278643]
Robot Arm 2 end: [-7.18781061514871;3.72628461504206;0.828807968179776]
Robot Arm 1 togoal: 14.0284863145788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001813e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-4.6094700914455;-6.3134730638694;-5.17886621695983]
Robot Arm 1 end: [-6.27724067168174;-4.50073402335041;1.11543395111102]
Robot Arm 1 togoal: 21.8033101826263
Mobile Platform 2 Pose: [3.54365080173727;-6.93276045841099;4.31909183370375]
Robot Arm 2 end: [-7.51388443305184;3.77730686142849;0.621561270639601]
Robot Arm 1 togoal: 14.0286829386817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007002e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-4.45645456899428;-6.37512268630196;-5.13431160348865]
Robot Arm 1 end: [-6.22135769192578;-4.47715375977386;1.15104167284385]
Robot Arm 1 togoal: 21.7452821438897
Mobile Platform 2 Pose: [3.84734989489855;-7.08770407194598;4.2816116499595]
Robot Arm 2 end: [-7.74193602606948;3.87618521484592;0.455953763415561]
Robot Arm 1 togoal: 14.0989961526746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.707109e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.017361e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-4.29477315957428;-6.44315811715954;-5.09976329537277]
Robot Arm 1 end: [-6.21059197259866;-4.54339537269195;1.25054024056521]
Robot Arm 1 togoal: 21.7799630291236
Mobile Platform 2 Pose: [4.098157334074;-7.22147191394274;4.25895617257483]
Robot Arm 2 end: [-7.93603373199884;3.96410303099083;0.473071086292291]
Robot Arm 1 togoal: 14.1546559556832

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.613918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.930975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-4.09248160453888;-6.49702608563971;-5.01179759203891]
Robot Arm 1 end: [-6.16668766481881;-4.46055037257506;1.1839464565161]
Robot Arm 1 togoal: 21.69292309074
Mobile Platform 2 Pose: [4.29481672985019;-7.33256057263751;4.24472729040603]
Robot Arm 2 end: [-8.07199716256879;4.1000685411667;0.398601056346192]
Robot Arm 1 togoal: 14.2754652181796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.048513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804285e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-3.94093782010635;-6.45238001930582;-4.8610834233808]
Robot Arm 1 end: [-6.00520834666419;-4.33494387050993;1.10143574142342]
Robot Arm 1 togoal: 21.4903081447864
Mobile Platform 2 Pose: [4.43285195943176;-7.41685378225363;4.24031213378909]
Robot Arm 2 end: [-8.15364911912062;4.20667764102543;0.370538847416587]
Robot Arm 1 togoal: 14.3722740731917

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.317842e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.120852e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-3.68650739102891;-6.47895452579524;-4.75021643814553]
Robot Arm 1 end: [-5.94315092000312;-4.3191098134787;1.08955388585771]
Robot Arm 1 togoal: 21.4337843802224
Mobile Platform 2 Pose: [4.48230451851384;-7.44214178041327;4.23805296676009]
Robot Arm 2 end: [-8.15109277257204;4.09086597764806;0.358347803557909]
Robot Arm 1 togoal: 14.2591276754774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.410063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.085012e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-3.6108834350097;-6.35607748080181;-4.57959900017016]
Robot Arm 1 end: [-5.76776465120819;-4.13249852556455;0.953803195883346]
Robot Arm 1 togoal: 21.1818702877148
Mobile Platform 2 Pose: [4.56202373970413;-7.48927648763556;4.23650520146436]
Robot Arm 2 end: [-8.18765231982811;4.109321010731;0.349563097490081]
Robot Arm 1 togoal: 14.2733951081809

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.310780e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.512962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-3.46228098561402;-6.2865782435471;-4.43428318176201]
Robot Arm 1 end: [-5.6149851438056;-3.98591266974167;0.761181301311155]
Robot Arm 1 togoal: 20.9764353192288
Mobile Platform 2 Pose: [4.59434691278962;-7.50363201694682;4.23373906700436]
Robot Arm 2 end: [-8.16549464049634;3.99425175004447;0.361001050568642]
Robot Arm 1 togoal: 14.1615708705056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.537668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-3.60076191553188;-6.0686570639609;-4.31281408499509]
Robot Arm 1 end: [-5.50010415913506;-3.74958978582075;0.691965086193711]
Robot Arm 1 togoal: 20.7362423575414
Mobile Platform 2 Pose: [4.65393263644609;-7.54089811525385;4.23357098816741]
Robot Arm 2 end: [-8.18006195270768;3.95229593828693;0.339606405176736]
Robot Arm 1 togoal: 14.1200009657014

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.658938e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.430838e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-3.47196730285565;-6.01352766154086;-4.19286197451539]
Robot Arm 1 end: [-5.33620102758055;-3.74385425372606;0.537444184133091]
Robot Arm 1 togoal: 20.6169826538452
Mobile Platform 2 Pose: [4.7272140050144;-7.59409793930659;4.23617561238623]
Robot Arm 2 end: [-8.22145391196661;3.95304250231206;0.310263187422556]
Robot Arm 1 togoal: 14.1179497409474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.723916e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.033031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-3.74695646709839;-5.79279835691667;-4.1303781163794]
Robot Arm 1 end: [-5.22472051191126;-3.57530022837364;0.75096249642836]
Robot Arm 1 togoal: 20.4125904459282
Mobile Platform 2 Pose: [4.80449814354694;-7.66564393529776;4.24738464257283]
Robot Arm 2 end: [-8.3079238815481;4.04439277238233;0.27770775878662]
Robot Arm 1 togoal: 14.2004882145876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.053050e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-3.58066376853372;-5.78062585861176;-4.03823459702012]
Robot Arm 1 end: [-5.09745231505288;-3.58691836669129;0.617987106797806]
Robot Arm 1 togoal: 20.3310808712601
Mobile Platform 2 Pose: [4.7606812296682;-7.65330252119076;4.2588498585627]
Robot Arm 2 end: [-8.26265572025538;3.92293110978926;0.332286039653483]
Robot Arm 1 togoal: 14.0811724197178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.904312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-3.68846316078075;-5.58267411717691;-3.93888489322837]
Robot Arm 1 end: [-4.9849621575348;-3.4042608719727;0.649105209136874]
Robot Arm 1 togoal: 20.1242018702105
Mobile Platform 2 Pose: [4.81692316211959;-7.73275385820791;4.2823668763881]
Robot Arm 2 end: [-8.30958530800913;3.9372632567229;0.275283174704633]
Robot Arm 1 togoal: 14.0945599195903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.120377e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-3.60848222742376;-5.49588582021075;-3.82722614763564]
Robot Arm 1 end: [-4.79127563843795;-3.41039664034701;0.614025944580345]
Robot Arm 1 togoal: 19.9860745270521
Mobile Platform 2 Pose: [5.06472405752894;-7.91954762864164;4.2957689187238]
Robot Arm 2 end: [-8.59043062093468;4.03865243360756;0.327356385842185]
Robot Arm 1 togoal: 14.1596409934593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.037467e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-3.76548114182376;-5.2811047696507;-3.74107402916955]
Robot Arm 1 end: [-4.68394917621541;-3.21076471004119;0.712986637251356]
Robot Arm 1 togoal: 19.7685495984943
Mobile Platform 2 Pose: [4.95132559959257;-7.85451389047153;4.2970995328972]
Robot Arm 2 end: [-8.44449672337174;3.93304828379232;0.324962101859345]
Robot Arm 1 togoal: 14.0705341610004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.054241e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-3.62150681687662;-5.25502895111325;-3.6560541610706]
Robot Arm 1 end: [-4.55071551830196;-3.23963787010211;0.60209143918405]
Robot Arm 1 togoal: 19.6940530609912
Mobile Platform 2 Pose: [4.78966435900852;-7.80666134064553;4.33109296895655]
Robot Arm 2 end: [-8.30870624042402;3.88841331034722;0.269888152703813]
Robot Arm 1 togoal: 14.0468423396383

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.268595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-3.79115066813937;-5.03775139385984;-3.57275725529362]
Robot Arm 1 end: [-4.44064553703108;-3.04336281734972;0.719707364700864]
Robot Arm 1 togoal: 19.475718908389
Mobile Platform 2 Pose: [5.10304827438242;-8.05258861447554;4.33760156661106]
Robot Arm 2 end: [-8.65971392269808;3.95421002768708;0.324799824999815]
Robot Arm 1 togoal: 14.0693723784852

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013480e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-3.645037789228;-5.01767347024694;-3.49248409139163]
Robot Arm 1 end: [-4.31305408456341;-3.07626992117941;0.605732175851726]
Robot Arm 1 togoal: 19.4085050292159
Mobile Platform 2 Pose: [5.05282506924398;-8.02561410670848;4.33999216641448]
Robot Arm 2 end: [-8.53751066062574;3.87054148066272;0.310431387270458]
Robot Arm 1 togoal: 13.9998668708204

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.028430e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-3.81053497879258;-4.80216405809852;-3.40889529336858]
Robot Arm 1 end: [-4.20483926113228;-2.87948166942323;0.717927783441756]
Robot Arm 1 togoal: 19.1912331936387
Mobile Platform 2 Pose: [4.95564767550484;-7.97669568663991;4.347621856663]
Robot Arm 2 end: [-8.38793712779663;3.80201124148008;0.298140474457939]
Robot Arm 1 togoal: 13.9495392910694

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.039230e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-3.66694123830507;-4.77983938088291;-3.32753692929954]
Robot Arm 1 end: [-4.07505234257935;-2.91158661131025;0.605977042317418]
Robot Arm 1 togoal: 19.1220666843519
Mobile Platform 2 Pose: [4.81338744863268;-7.91250173352082;4.36125083550921]
Robot Arm 2 end: [-8.21480477023089;3.74497445449277;0.305420136439081]
Robot Arm 1 togoal: 13.9136220721904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.046407e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-3.83187807464163;-4.56525071738372;-3.24437167524124]
Robot Arm 1 end: [-3.96776771656203;-2.71514595826723;0.717994457808588]
Robot Arm 1 togoal: 18.9055095044979
Mobile Platform 2 Pose: [4.65380297661136;-7.85065763738132;4.37574115664767]
Robot Arm 2 end: [-8.02961575930849;3.70862658481549;0.315958165972648]
Robot Arm 1 togoal: 13.9018337898146

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.034828e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-3.6898911774442;-4.54242063830794;-3.16318222163925]
Robot Arm 1 end: [-3.8374278679181;-2.74748898391738;0.607116509690391]
Robot Arm 1 togoal: 18.836349657744
Mobile Platform 2 Pose: [4.50503768160223;-7.80013082244306;4.38511592379634]
Robot Arm 2 end: [-7.84833207610439;3.69442413577378;0.320185380854217]
Robot Arm 1 togoal: 13.9143409119778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.030418e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-3.85331541009282;-4.32901784704478;-3.08027780258991]
Robot Arm 1 end: [-3.73136105286339;-2.55119285091233;0.717621745531156]
Robot Arm 1 togoal: 18.6206373404521
Mobile Platform 2 Pose: [4.36631630939081;-7.75466917943294;4.38912472811002]
Robot Arm 2 end: [-7.66918442730127;3.69993278715586;0.326075742291413]
Robot Arm 1 togoal: 13.9480716799199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.021925e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-3.71318403728312;-4.30524091292873;-2.99893767928092]
Robot Arm 1 end: [-3.60004630210982;-2.58350988994115;0.608104689877046]
Robot Arm 1 togoal: 18.5509934617266
Mobile Platform 2 Pose: [4.22445637647014;-7.70807177794998;4.38790918316294]
Robot Arm 2 end: [-7.49146597659519;3.72621018996259;0.331499409151737]
Robot Arm 1 togoal: 14.0041601240932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.019268e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-3.87525310549199;-4.09301313784464;-2.9163433890449]
Robot Arm 1 end: [-3.49518465797933;-2.38741319868589;0.71735852083828]
Robot Arm 1 togoal: 18.3361352071522
Mobile Platform 2 Pose: [4.0725300154087;-7.65589235231592;4.38029410370559]
Robot Arm 2 end: [-7.31680959485274;3.77067317436223;0.335922769297617]
Robot Arm 1 togoal: 14.079607894686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012350e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-3.73694688792072;-4.06837461833593;-2.83491575409483]
Robot Arm 1 end: [-3.36297311692653;-2.41975817395812;0.609179493891993]
Robot Arm 1 togoal: 18.2661123466013
Mobile Platform 2 Pose: [3.90857483750582;-7.59411124489107;4.36569753340665]
Robot Arm 2 end: [-7.1449241288368;3.82750339166065;0.346171730290421]
Robot Arm 1 togoal: 14.1679754989834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-3.89756561949771;-3.85737663148039;-2.75263456142073]
Robot Arm 1 end: [-3.25730964755737;-2.22472060785234;0.706011575132053]
Robot Arm 1 togoal: 18.0517106990982
Mobile Platform 2 Pose: [3.73295818559204;-7.51934371066028;4.34468851405217]
Robot Arm 2 end: [-6.97488617765431;3.89520743729535;0.359857504771603]
Robot Arm 1 togoal: 14.2680186170975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.032179e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-3.7927752506566;-3.80455824416435;-2.66328833563545]
Robot Arm 1 end: [-3.10160908660524;-2.25311761032521;0.547638047692549]
Robot Arm 1 togoal: 17.9649237493756
Mobile Platform 2 Pose: [3.5586848243935;-7.43426004765809;4.32214693833293]
Robot Arm 2 end: [-6.83927995960682;3.9606760054962;0.324928775310286]
Robot Arm 1 togoal: 14.3638856887233

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.575269e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-4.04178943130742;-3.58484901373555;-2.60495082535804]
Robot Arm 1 end: [-2.9879160531629;-2.08444643165191;0.761004475547732]
Robot Arm 1 togoal: 17.7566157293742
Mobile Platform 2 Pose: [3.41154278621198;-7.35558616448519;4.30455857853398]
Robot Arm 2 end: [-6.77166612927955;3.92183772634691;0.308089469619245]
Robot Arm 1 togoal: 14.3426237911438

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.679749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-3.91572069786661;-3.56425489410635;-2.53413537250978]
Robot Arm 1 end: [-2.85934349462924;-2.12273560954814;0.58039657449772]
Robot Arm 1 togoal: 17.6976882268825
Mobile Platform 2 Pose: [3.2674065453282;-7.27867118939589;4.28619483424183]
Robot Arm 2 end: [-6.69890174010678;3.88800219810444;0.297888720657112]
Robot Arm 1 togoal: 14.3272417435346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.431416e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.943872e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-4.12682396389416;-3.34260323591189;-2.46159216480111]
Robot Arm 1 end: [-2.75409209308256;-1.93147348342381;0.751180399638233]
Robot Arm 1 togoal: 17.481977772025
Mobile Platform 2 Pose: [3.15733735321198;-7.22285742711603;4.27423268515856]
Robot Arm 2 end: [-6.61328437183227;3.86470167172385;0.35211984826877]
Robot Arm 1 togoal: 14.3201283514181

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.404536e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-3.99063501708607;-3.33339122297833;-2.39357417946399]
Robot Arm 1 end: [-2.63405687672454;-1.97198624824879;0.602146340419919]
Robot Arm 1 togoal: 17.4296339588728
Mobile Platform 2 Pose: [3.05257423201967;-7.16962862536357;4.26453382391735]
Robot Arm 2 end: [-6.53279846325182;3.84216992623779;0.41900720671509]
Robot Arm 1 togoal: 14.312286430886
Step 177:
Mobile Platform 1 Pose: [-4.19501837502002;-3.10889206447028;-2.31222702243539]
Robot Arm 1 end: [-2.38289057037325;-1.97902138476235;0.677257479178412]
Robot Arm 1 togoal: 17.2494879244537
Mobile Platform 2 Pose: [2.94722545267697;-7.11493583030951;4.25535371552519]
Robot Arm 2 end: [-6.45487562921815;3.82523020481436;0.492059490288706]
Robot Arm 1 togoal: 14.309563780666
Step 178:
Mobile Platform 1 Pose: [-4.39120429226429;-2.95943241007106;-2.29230877609935]
Robot Arm 1 end: [-2.2902497571303;-1.92401479150584;0.793431443396974]
Robot Arm 1 togoal: 17.1394785173076
Mobile Platform 2 Pose: [2.84070692017275;-7.05802903317368;4.24639027215001]
Robot Arm 2 end: [-6.38195391538797;3.8118288146231;0.568742539800672]
Robot Arm 1 togoal: 14.3095655750038
Step 179:
Mobile Platform 1 Pose: [-4.5524647579762;-2.78043376614779;-2.24329242053532]
Robot Arm 1 end: [-2.15295685969383;-1.90994333729462;0.935229595587854]
Robot Arm 1 togoal: 17.0259871284408
Mobile Platform 2 Pose: [2.73331441770305;-6.99867028929914;4.23760232925982]
Robot Arm 2 end: [-6.31693380940044;3.79850870364797;0.646794545214119]
Robot Arm 1 togoal: 14.3083211519089

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','9.769963e-15','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-4.54337479015558;-2.69715280098251;-2.19945771341669]
Robot Arm 1 end: [-1.9678360307236;-1.97655535970455;0.834774424351198]
Robot Arm 1 togoal: 16.9451433479634
Mobile Platform 2 Pose: [2.88748757871766;-7.06755876148139;4.22902216181289]
Robot Arm 2 end: [-6.32010555336411;3.90864465011265;0.614759151856044]
Robot Arm 1 togoal: 14.4157307000489

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.957935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-4.70184269213617;-2.52786323452226;-2.16736930740966]
Robot Arm 1 end: [-1.86820408976902;-2.00465805230137;1.01835413171509]
Robot Arm 1 togoal: 16.8884201788681
Mobile Platform 2 Pose: [2.91044474068648;-7.12723605938212;4.22784544581449]
Robot Arm 2 end: [-6.44131281240843;3.80594553753811;0.559599153291992]
Robot Arm 1 togoal: 14.2896003538464

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.255922e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-4.65729861898536;-2.48644638918448;-2.14487995567644]
Robot Arm 1 end: [-1.72729870004754;-2.11744793578861;0.878551679077273]
Robot Arm 1 togoal: 16.8752724162231
Mobile Platform 2 Pose: [2.85318305012615;-7.16834252267472;4.2235248570501]
Robot Arm 2 end: [-6.54083225404735;3.73116586066861;0.482376372710603]
Robot Arm 1 togoal: 14.1982215938122

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.268808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-4.78469071228373;-2.33089097087985;-2.12474186462277]
Robot Arm 1 end: [-1.66483078738539;-2.14723123644231;1.06355014290582]
Robot Arm 1 togoal: 16.8473370115781
Mobile Platform 2 Pose: [2.67950875177237;-7.06599382038384;4.2122020432899]
Robot Arm 2 end: [-6.5018222090803;3.69730030950414;0.352253890640262]
Robot Arm 1 togoal: 14.1851771611697

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988173e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-4.72574950537573;-2.2965162077889;-2.10952507015747]
Robot Arm 1 end: [-1.55380008336338;-2.28153048532488;0.907990089788514]
Robot Arm 1 togoal: 16.8731167416784
Mobile Platform 2 Pose: [2.7081570766273;-7.03653887916231;4.20424531462233]
Robot Arm 2 end: [-6.53729971806744;3.7190455167322;0.272517028484383]
Robot Arm 1 togoal: 14.2042674173232

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994299e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-4.87691575109614;-2.13387620003954;-2.10132603994164]
Robot Arm 1 end: [-1.49627433466974;-2.25435694029428;1.08755389175836]
Robot Arm 1 togoal: 16.8083747913614
Mobile Platform 2 Pose: [2.61004362091473;-6.94784903319354;4.19757515762172]
Robot Arm 2 end: [-6.50748260303594;3.8052166888036;0.169394294190054]
Robot Arm 1 togoal: 14.3041435806865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-4.82088804912463;-2.08851093387712;-2.08979247286457]
Robot Arm 1 end: [-1.41418541963835;-2.39098084652818;0.97776861020616]
Robot Arm 1 togoal: 16.8557152169752
Mobile Platform 2 Pose: [2.55360702676721;-6.89853550267065;4.19468043045843]
Robot Arm 2 end: [-6.53421348630951;3.91451220713509;0.0914905158271709]
Robot Arm 1 togoal: 14.4107190482061

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-4.98346291132508;-1.93213237062824;-2.09220397814273]
Robot Arm 1 end: [-1.35376771381729;-2.28959596894052;1.14688433554549]
Robot Arm 1 togoal: 16.7356557358685
Mobile Platform 2 Pose: [2.52434986028224;-6.87116958543476;4.18702125331051]
Robot Arm 2 end: [-6.59415648573597;4.02448466956526;-0.00952644680951981]
Robot Arm 1 togoal: 14.5130441842816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-4.94024521816816;-1.86469103241985;-2.08199315277705]
Robot Arm 1 end: [-1.30743900634388;-2.40707212769112;1.13324684357918]
Robot Arm 1 togoal: 16.7911872175326
Mobile Platform 2 Pose: [2.53755196073713;-6.87935734011333;4.15497754633687]
Robot Arm 2 end: [-6.66233801767463;4.14581466765586;-0.0633874525738565]
Robot Arm 1 togoal: 14.6203400685415

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-4.98668452773343;-1.73686848017534;-2.08434134919494]
Robot Arm 1 end: [-1.26240239074574;-2.48222882931297;1.16727577846177]
Robot Arm 1 togoal: 16.8158515985159
Mobile Platform 2 Pose: [2.61570482342169;-6.92330085475273;4.10464112466919]
Robot Arm 2 end: [-6.79062419693081;4.2280415311758;-0.0811790642286374]
Robot Arm 1 togoal: 14.6732490134953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814786e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-4.94796596365016;-1.6911811796828;-2.08969194700434]
Robot Arm 1 end: [-1.25091821555399;-2.63666610858948;1.08360409474551]
Robot Arm 1 togoal: 16.925123521292
Mobile Platform 2 Pose: [2.78840045033443;-7.00890497500588;4.03996386498231]
Robot Arm 2 end: [-6.98579084455713;4.24471943622032;-0.0681415557523499]
Robot Arm 1 togoal: 14.6465989066953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-5.0030428780168;-1.58120926036272;-2.10883218290011]
Robot Arm 1 end: [-1.24357254200517;-2.71438804603806;1.10754721716693]
Robot Arm 1 togoal: 16.977765756901
Mobile Platform 2 Pose: [3.04619076506298;-7.12699004936522;3.96198490921852]
Robot Arm 2 end: [-7.1851434728974;4.11173245492468;-0.0662598518843009]
Robot Arm 1 togoal: 14.4770094973012

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-4.9530617634708;-1.55959484916131;-2.12149583786996]
Robot Arm 1 end: [-1.25338596086411;-2.86370428858378;0.922566768861559]
Robot Arm 1 togoal: 17.1018041720876
Mobile Platform 2 Pose: [3.21918309275804;-7.20928669629314;3.92765158946434]
Robot Arm 2 end: [-7.33214765574635;3.98210207705578;-0.056410250617052]
Robot Arm 1 togoal: 14.3214816874058

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-5.09417715633895;-1.41172852159218;-2.15461705940676]
Robot Arm 1 end: [-1.25197962337596;-2.88179580229459;1.05094048530881]
Robot Arm 1 togoal: 17.110484051722
Mobile Platform 2 Pose: [3.36559647530085;-7.27910869656415;3.89632775646707]
Robot Arm 2 end: [-7.42389872594676;3.80547659737027;-0.031541866253316]
Robot Arm 1 togoal: 14.1293265268089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989355e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-5.04720541484746;-1.41977801928883;-2.16012930438188]
Robot Arm 1 end: [-1.24142905924366;-2.96603687280427;0.899841658573428]
Robot Arm 1 togoal: 17.1718862436848
Mobile Platform 2 Pose: [3.73290658271447;-7.45478369936342;3.77817265403928]
Robot Arm 2 end: [-7.52991983017858;3.63897582140079;-0.0513940393993766]
Robot Arm 1 togoal: 13.9497448190725

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.665732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.892933e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-4.92537880885291;-1.40130067390662;-2.19188797848054]
Robot Arm 1 end: [-1.13848697925542;-3.02152971235891;0.723211190381548]
Robot Arm 1 togoal: 17.1540839225428
Mobile Platform 2 Pose: [3.84842218454474;-7.54406178087029;3.71736457677776]
Robot Arm 2 end: [-7.58248338818326;3.48057453050511;-0.0376525534471157]
Robot Arm 1 togoal: 13.7840369276667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.077364e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.851529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-4.70398921986769;-1.35968922690521;-2.22851119277648]
Robot Arm 1 end: [-0.929913167812066;-2.95644836316915;0.519551512545379]
Robot Arm 1 togoal: 16.9804433133062
Mobile Platform 2 Pose: [3.87973379385844;-7.61520472767923;3.58699148364717]
Robot Arm 2 end: [-7.64700147467363;3.22176001488884;0.00978251516339551]
Robot Arm 1 togoal: 13.5142635107496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.889234e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-4.92386371385047;-1.21380263243648;-2.2457807761375]
Robot Arm 1 end: [-0.99993340228247;-3.07929920418165;0.462958327483234]
Robot Arm 1 togoal: 17.1226766221855
Mobile Platform 2 Pose: [3.75380919919807;-7.62944067575695;3.5181253009657]
Robot Arm 2 end: [-7.54100959096057;2.99911354903456;0.12340090982092]
Robot Arm 1 togoal: 13.3032653927264

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.330499e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000931e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-5.25739314215981;-1.02874969009803;-2.27064962723371]
Robot Arm 1 end: [-0.996171263259511;-3.14608223668862;0.563139088583489]
Robot Arm 1 togoal: 17.1654524598198
Mobile Platform 2 Pose: [3.52178867857406;-7.53814753570694;3.46485231199804]
Robot Arm 2 end: [-7.31180287080162;2.85807423886519;0.2650547925641]
Robot Arm 1 togoal: 13.1959795127789

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.204851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001088e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-5.87052334665225;-0.649239342996068;-2.35132183333497]
Robot Arm 1 end: [-0.825210388563584;-3.01118713636488;0.331330691576375]
Robot Arm 1 togoal: 16.9673652556187
Mobile Platform 2 Pose: [3.27264218536219;-7.40272247939586;3.40943593263805]
Robot Arm 2 end: [-7.08774519470753;2.77277490608297;0.359431048259639]
Robot Arm 1 togoal: 13.1519675061209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001025e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996586e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-5.96585331859762;-0.609074379601737;-2.35973844830619]
Robot Arm 1 end: [-0.853023379778394;-3.01257775345278;0.287644548272443]
Robot Arm 1 togoal: 16.9893043395537
Mobile Platform 2 Pose: [2.99850920297273;-7.22140183285666;3.34354685868796]
Robot Arm 2 end: [-6.77634582564143;2.72859265749538;0.487534584454421]
Robot Arm 1 togoal: 13.1710693583108

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000884e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-6.10016948987084;-0.555934203576277;-2.37496141870461]
Robot Arm 1 end: [-0.888623347262765;-2.99777866206439;0.253884929911273]
Robot Arm 1 togoal: 17.0032334909398
Mobile Platform 2 Pose: [2.83639548311637;-7.06413920059199;3.30567807531949]
Robot Arm 2 end: [-6.49882327303989;2.7829955577994;0.609409120650617]
Robot Arm 1 togoal: 13.2851124133535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002779e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-6.24396286432637;-0.506311320901717;-2.39654384036943]
Robot Arm 1 end: [-0.881034939022898;-2.9543882240113;0.171782673104915]
Robot Arm 1 togoal: 16.9715492220903
Mobile Platform 2 Pose: [2.65228601728953;-7.00741802378057;3.28397988210035]
Robot Arm 2 end: [-6.37871456807478;2.85996697779431;0.715575634165396]
Robot Arm 1 togoal: 13.3843627886618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002797e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996120e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-6.35169349073092;-0.485891252468852;-2.43535875753493]
Robot Arm 1 end: [-0.919056694252695;-2.94115742912024;0.129964425370494]
Robot Arm 1 togoal: 16.9892417331517
Mobile Platform 2 Pose: [2.2979493648581;-6.83462395255859;3.2492983885826]
Robot Arm 2 end: [-6.13629000237107;3.02910487998825;0.870555496543762]
Robot Arm 1 togoal: 13.6054734196054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.900907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952105e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-6.33218260327644;-0.559208741197771;-2.51411764869244]
Robot Arm 1 end: [-1.02030431917901;-2.99245327503121;0.120897421259973]
Robot Arm 1 togoal: 17.0941872173607
Mobile Platform 2 Pose: [2.01295846286985;-6.67140514139615;3.24633824501825]
Robot Arm 2 end: [-5.9585319703649;3.24853890061588;0.926051763003134]
Robot Arm 1 togoal: 13.8640257587903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969391e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-6.10909121270186;-0.762743050483816;-2.63087332551018]
Robot Arm 1 end: [-1.27707382583606;-3.07342345722257;0.131300188802119]
Robot Arm 1 togoal: 17.3210294304483
Mobile Platform 2 Pose: [1.84135402613371;-6.57088694776806;3.25594688585902]
Robot Arm 2 end: [-5.85271400644798;3.45514469478465;0.804674504014462]
Robot Arm 1 togoal: 14.0980148278438

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000623e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989768e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-5.87862766039137;-0.948809240977873;-2.69104896274062]
Robot Arm 1 end: [-1.5414746866982;-3.06729256622202;0.155823483649482]
Robot Arm 1 togoal: 17.4878094647595
Mobile Platform 2 Pose: [1.79925079641193;-6.54314353183213;3.25922254431801]
Robot Arm 2 end: [-5.83896759166419;3.54111911877589;0.616807595147018]
Robot Arm 1 togoal: 14.1948463041025

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001765e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-5.62213016106362;-1.1444711235026;-2.73319226195692]
Robot Arm 1 end: [-1.79745591520808;-2.98137165918716;0.147524113691644]
Robot Arm 1 togoal: 17.594954169747
Mobile Platform 2 Pose: [1.7909153542735;-6.53471889683451;3.25320728914418]
Robot Arm 2 end: [-5.80449353178018;3.48611172941478;0.437469168571186]
Robot Arm 1 togoal: 14.1651517170057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997005e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005095e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-5.39386243721176;-1.29888445823062;-2.74804573071196]
Robot Arm 1 end: [-1.98604865429479;-2.86448135887829;0.144551348867732]
Robot Arm 1 togoal: 17.636747258643
Mobile Platform 2 Pose: [1.74535070240775;-6.50952969790845;3.23546417053844]
Robot Arm 2 end: [-5.80032452085679;3.43799377834269;0.306881392593161]
Robot Arm 1 togoal: 14.1312078361952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-5.20299619405565;-1.41951602088042;-2.75182079995747]
Robot Arm 1 end: [-2.11240642472405;-2.79381159208756;0.142088800958493]
Robot Arm 1 togoal: 17.6718250485447
Mobile Platform 2 Pose: [1.63923432543513;-6.45380665257455;3.20740679707194]
Robot Arm 2 end: [-5.78050350425014;3.32807822184296;0.190899628820607]
Robot Arm 1 togoal: 14.0431829284753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993913e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.834977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-5.03311972686831;-1.52360473859181;-2.75252366080745]
Robot Arm 1 end: [-2.21298640558841;-2.78444179515346;0.138822038527376]
Robot Arm 1 togoal: 17.7344130118235
Mobile Platform 2 Pose: [1.47989338053696;-6.37429004321132;3.17732873110131]
Robot Arm 2 end: [-5.64346316448938;3.08155619597299;0.281095868075771]
Robot Arm 1 togoal: 13.8435504103508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996154e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010675e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-4.87173215537875;-1.62123075716203;-2.75307607638958]
Robot Arm 1 end: [-2.30589112327838;-2.78076308384586;0.134545339285645]
Robot Arm 1 togoal: 17.7962107727908
Mobile Platform 2 Pose: [1.41176950509781;-6.33773608977169;3.17227549267596]
Robot Arm 2 end: [-5.67742884278337;2.8965738463628;0.267823651347585]
Robot Arm 1 togoal: 13.6593077915651

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999677e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.610634e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-4.71465165742407;-1.71549679315216;-2.75359880265968]
Robot Arm 1 end: [-2.39593499472927;-2.69581824061599;0.132840398944205]
Robot Arm 1 togoal: 17.7980333820991
Mobile Platform 2 Pose: [1.28216489647162;-6.26672095939724;3.16197558174128]
Robot Arm 2 end: [-5.71552341332709;2.74540830524183;0.26309936049885]
Robot Arm 1 togoal: 13.5049800632186

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999777e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.281507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-4.56332770841035;-1.80506763556079;-2.7519328546108]
Robot Arm 1 end: [-2.45690679690055;-2.67443915581415;0.111776643759486]
Robot Arm 1 togoal: 17.826997653247
Mobile Platform 2 Pose: [1.02420130862009;-6.10902349792342;3.1329824119686]
Robot Arm 2 end: [-5.66684839341757;2.62444465962367;0.276660235675092]
Robot Arm 1 togoal: 13.4052671461112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000617e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-4.40230078360988;-1.90128461296228;-2.7488186352285]
Robot Arm 1 end: [-2.55920783928382;-2.74235208090705;0.112151835100634]
Robot Arm 1 togoal: 17.9467571236662
Mobile Platform 2 Pose: [0.781502138564369;-5.86787600551483;3.06704390339291]
Robot Arm 2 end: [-5.47865752929368;2.51292563974193;0.293565650390431]
Robot Arm 1 togoal: 13.3612290185344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000505e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-4.24051503487633;-1.9971713597435;-2.74804663101282]
Robot Arm 1 end: [-2.61655388290953;-2.66830417121744;0.0864485611933789]
Robot Arm 1 togoal: 17.9365910999116
Mobile Platform 2 Pose: [1.05531457723749;-5.82349462580036;3.03238357583025]
Robot Arm 2 end: [-5.3794765588956;2.52954328327618;0.254249175465622]
Robot Arm 1 togoal: 13.4142964559736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001990e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-4.05313058361966;-2.11059227774321;-2.74192115281374]
Robot Arm 1 end: [-2.77133860479623;-2.73956992760479;0.115658836777916]
Robot Arm 1 togoal: 18.0936097969349
Mobile Platform 2 Pose: [1.29736342053265;-5.90362021803294;3.03979854508763]
Robot Arm 2 end: [-5.33326120281596;2.67892097081711;0.225778575054794]
Robot Arm 1 togoal: 13.5724384885328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001227e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-3.89669122902293;-2.20329102434189;-2.74112300801367]
Robot Arm 1 end: [-2.86143703347473;-2.73291158383846;0.113092961201807]
Robot Arm 1 togoal: 18.1528455675076
Mobile Platform 2 Pose: [1.45321617416777;-5.9627635133146;3.04806994992041]
Robot Arm 2 end: [-5.31488578625665;2.80201892047794;0.217800225117835]
Robot Arm 1 togoal: 13.6945358917105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001652e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-3.74185612989842;-2.295062926851;-2.74009639579462]
Robot Arm 1 end: [-2.95206002289148;-2.7279433198741;0.111941667246975]
Robot Arm 1 togoal: 18.2137817428345
Mobile Platform 2 Pose: [1.56349379036174;-6.00977880515295;3.05442474412012]
Robot Arm 2 end: [-5.31295025928373;2.93138517741871;0.216013321171368]
Robot Arm 1 togoal: 13.8163724656884

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002280e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-3.58936585745935;-2.38549944446397;-2.73891586328606]
Robot Arm 1 end: [-3.04277176796875;-2.72470645289377;0.112013039767251]
Robot Arm 1 togoal: 18.2761400180056
Mobile Platform 2 Pose: [1.64728354284725;-6.04775398568062;3.05937920459736]
Robot Arm 2 end: [-5.32915782495976;3.06127603783783;0.216863810549906]
Robot Arm 1 togoal: 13.9324968889774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.731955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-3.44513883927901;-2.47156681684851;-2.73767323619885]
Robot Arm 1 end: [-3.13233602395251;-2.72599335668613;0.114490741831024]
Robot Arm 1 togoal: 18.3408676090782
Mobile Platform 2 Pose: [1.70599459371464;-6.07614531315544;3.06276393336178]
Robot Arm 2 end: [-5.34997626551517;3.06903406247768;0.219844735298159]
Robot Arm 1 togoal: 13.9325294573647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.949443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-3.30518163385894;-2.55491957979769;-2.73664545268673]
Robot Arm 1 end: [-3.21821762812797;-2.72618251580203;0.116333172208259]
Robot Arm 1 togoal: 18.4024478694318
Mobile Platform 2 Pose: [1.76166321351473;-6.10428392105006;3.06598153455494]
Robot Arm 2 end: [-5.37315479059021;3.07082061578231;0.223853083400795]
Robot Arm 1 togoal: 13.9261135384227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.243758e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-3.16849863574215;-2.63621433194735;-2.73573839215502]
Robot Arm 1 end: [-3.31316184672713;-2.74684244107604;0.126651415383833]
Robot Arm 1 togoal: 18.4842224221142
Mobile Platform 2 Pose: [1.81495525020109;-6.13176468964042;3.06915326964879]
Robot Arm 2 end: [-5.39820338490676;3.07209498707775;0.228606868821662]
Robot Arm 1 togoal: 13.9185659980971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.776899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-3.04363551634086;-2.70975416337034;-2.73592746993895]
Robot Arm 1 end: [-3.3919153961667;-2.75534711255909;0.13103977344218]
Robot Arm 1 togoal: 18.5465432728411
Mobile Platform 2 Pose: [1.93669088089496;-6.19366174817186;3.07232293674657]
Robot Arm 2 end: [-5.4701248709435;3.0743170604972;0.216644476553551]
Robot Arm 1 togoal: 13.8981609795972

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-2.92356981089797;-2.78022174286189;-2.73644297733378]
Robot Arm 1 end: [-3.46525744827273;-2.76018121728749;0.133589201237679]
Robot Arm 1 togoal: 18.6026958146072
Mobile Platform 2 Pose: [1.98836815897455;-6.22059480635624;3.0757513659278]
Robot Arm 2 end: [-5.50290859081607;3.05286600134252;0.217165004342747]
Robot Arm 1 togoal: 13.8672682128622

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-2.80628912486859;-2.84893957385727;-2.73710313198175]
Robot Arm 1 end: [-3.53579926031458;-2.76329515668189;0.135266834878028]
Robot Arm 1 togoal: 18.6558253979027
Mobile Platform 2 Pose: [2.15085784069666;-6.30282547662948;3.07994163037188]
Robot Arm 2 end: [-5.55428783521166;3.15003124351098;0.328083060451392]
Robot Arm 1 togoal: 13.9323626561371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999780e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-2.69089038637889;-2.91649901464358;-2.73783168480106]
Robot Arm 1 end: [-3.60463250857872;-2.76549413507167;0.136478205452352]
Robot Arm 1 togoal: 18.7072383532579
Mobile Platform 2 Pose: [2.16125428675875;-6.3093311502755;3.07999827839562]
Robot Arm 2 end: [-5.60395260941173;3.17876389875653;0.290070348673421]
Robot Arm 1 togoal: 13.9470512426814

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.728526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-2.576901735022;-2.98320864506196;-2.73859015171163]
Robot Arm 1 end: [-3.67230412244365;-2.76718085388872;0.137426248195647]
Robot Arm 1 togoal: 18.7575873528334
Mobile Platform 2 Pose: [2.1453639162906;-6.3023514117159;3.07872954199862]
Robot Arm 2 end: [-5.60461072954025;3.16950625219716;0.342745989418994]
Robot Arm 1 togoal: 13.9335431344286

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-2.46405708161263;-3.04924234062387;-2.73935737829475]
Robot Arm 1 end: [-3.73911364516862;-2.76857610424526;0.138221037099706]
Robot Arm 1 togoal: 18.8072288534223
Mobile Platform 2 Pose: [2.12523902833657;-6.29242169900731;3.07602338050079]
Robot Arm 2 end: [-5.60496995553317;3.16502144442037;0.41271751388747]
Robot Arm 1 togoal: 13.9234467281933

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-2.35219935696874;-3.11470262531191;-2.74012124689942]
Robot Arm 1 end: [-3.80523367458975;-2.7698070189575;0.138924754848606]
Robot Arm 1 togoal: 18.8563670969438
Mobile Platform 2 Pose: [2.12966994188505;-6.29357503739433;3.07301518876808]
Robot Arm 2 end: [-5.621907581411;3.06284966516354;0.408850025842705]
Robot Arm 1 togoal: 13.8218165291465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984299e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-2.24123270737661;-3.17965193220513;-2.74087470318882]
Robot Arm 1 end: [-3.87076700146189;-2.77094879566071;0.139573239551275]
Robot Arm 1 togoal: 18.9051223282135
Mobile Platform 2 Pose: [2.16085921406541;-6.30980502985517;3.07078936209156]
Robot Arm 2 end: [-5.65554599024967;3.06885929868018;0.389402069150947]
Robot Arm 1 togoal: 13.8184614877575

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-2.13109681905353;-3.24412938833129;-2.74161365063867]
Robot Arm 1 end: [-3.93577644021866;-2.77204653715537;0.140187289266464]
Robot Arm 1 togoal: 18.9535664580875
Mobile Platform 2 Pose: [2.14592506468078;-6.30188685886388;3.06810109461624]
Robot Arm 2 end: [-5.6861839896939;2.95066504412026;0.366972577740912]
Robot Arm 1 togoal: 13.6989237879148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999999e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-2.02175242237103;-3.30816029461508;-2.7423357636372]
Robot Arm 1 end: [-4.000301347972;-2.77312743088788;0.140778925177778]
Robot Arm 1 togoal: 19.0017428783549
Mobile Platform 2 Pose: [2.16105981493498;-6.31538045140849;3.06503639140305]
Robot Arm 2 end: [-5.73749513014709;2.82881563848676;0.350399721390213]
Robot Arm 1 togoal: 13.5690000907983

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-1.91317286346486;-3.37176164273188;-2.74303979263687]
Robot Arm 1 end: [-4.06436710804108;-2.77420781536775;0.141354980472688]
Robot Arm 1 togoal: 19.0496778933361
Mobile Platform 2 Pose: [2.2032293317829;-6.35869548181542;3.05963878317907]
Robot Arm 2 end: [-5.7620182482741;2.88162269110414;0.341952274222142]
Robot Arm 1 togoal: 13.6120110940152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993906e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-1.80533912087158;-3.43494538121882;-2.74372514796828]
Robot Arm 1 end: [-4.12799069145587;-2.77529737556772;0.141919198662209]
Robot Arm 1 togoal: 19.0973874618669
Mobile Platform 2 Pose: [2.23731180570806;-6.38681410815331;3.05488657449729]
Robot Arm 2 end: [-5.7519689959191;2.94821139381508;0.39043568334986]
Robot Arm 1 togoal: 13.674067477698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-1.69823683082136;-3.49772036844622;-2.74439164687262]
Robot Arm 1 end: [-4.19118396578649;-2.77640168453634;0.142473481852446]
Robot Arm 1 togoal: 19.1448812405994
Mobile Platform 2 Pose: [2.27942051484542;-6.41803111721181;3.05288794357993]
Robot Arm 2 end: [-5.80256709549623;2.80560025094743;0.409594595776705]
Robot Arm 1 togoal: 13.5217255832017

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.663351e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-1.59185450540359;-3.56009354499496;-2.74503935852777]
Robot Arm 1 end: [-4.25395567616167;-2.77752375619376;0.143018639422219]
Robot Arm 1 togoal: 19.1921650260771
Mobile Platform 2 Pose: [2.42491309389098;-6.53167909083133;3.05440140200276]
Robot Arm 2 end: [-5.88912545973119;2.85235335243994;0.413844511904376]
Robot Arm 1 togoal: 13.5391310451086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-1.48618246400698;-3.62207063870911;-2.74566850853467]
Robot Arm 1 end: [-4.31631263403625;-2.77866499466133;0.143554838292042]
Robot Arm 1 togoal: 19.239242231823
Mobile Platform 2 Pose: [2.49996905904505;-6.59260900263157;3.0526307665765]
Robot Arm 2 end: [-5.97465753933917;2.7496789743861;0.442523727598637]
Robot Arm 1 togoal: 13.4134561894254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.768794e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-1.38121219103288;-3.68365659007301;-2.74627941953078]
Robot Arm 1 end: [-4.37826043723413;-2.77982578984475;0.144081879098917]
Robot Arm 1 togoal: 19.2861147961786
Mobile Platform 2 Pose: [2.60585016912558;-6.74377505830428;3.04824531429216]
Robot Arm 2 end: [-6.12773958953729;2.76187773008334;0.404746250235102]
Robot Arm 1 togoal: 13.3830469743589

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.223609e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-1.27693595132352;-3.74485580818734;-2.74687247418191]
Robot Arm 1 end: [-4.43980390449401;-2.78100588932669;0.144599364865718]
Robot Arm 1 togoal: 19.3327837377767
Mobile Platform 2 Pose: [2.52099593562554;-6.80340950168483;3.00191332632912]
Robot Arm 2 end: [-6.16085549922127;2.74934028813974;0.440695189554271]
Robot Arm 1 togoal: 13.3585840104449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.765274e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-1.17334655978039;-3.8056723251751;-2.74744809192634]
Robot Arm 1 end: [-4.50094733905555;-2.78220463401011;0.1451068048067]
Robot Arm 1 togoal: 19.3792494983771
Mobile Platform 2 Pose: [2.44559183931008;-6.81289856306863;2.96669453630747]
Robot Arm 2 end: [-6.1963969408831;2.64922055119088;0.447624884242408]
Robot Arm 1 togoal: 13.2522568237686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.007904e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-1.07043724364923;-3.86610988938253;-2.74800671425737]
Robot Arm 1 end: [-4.56169468740391;-2.78342110878222;0.145603678072189]
Robot Arm 1 togoal: 19.4255121542779
Mobile Platform 2 Pose: [2.29658326807829;-6.86361416465804;2.89830635531423]
Robot Arm 2 end: [-6.23684772703873;2.599061928039;0.448994080657777]
Robot Arm 1 togoal: 13.192682562207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.634121e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-0.968201557900152;-3.92617202336108;-2.74854879518016]
Robot Arm 1 end: [-4.62204963893346;-2.78465424164204;0.146089473222544]
Robot Arm 1 togoal: 19.4715715513261
Mobile Platform 2 Pose: [2.15473615266413;-6.87436396368026;2.84088095700236]
Robot Arm 2 end: [-6.22956870819378;2.54329061608327;0.509721642248092]
Robot Arm 1 togoal: 13.1367033734728

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.127581e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-0.866633334800517;-3.9858620591007;-2.74907479509092]
Robot Arm 1 end: [-4.68201568527884;-2.78590286848299;0.146563711848134]
Robot Arm 1 togoal: 19.5174273882406
Mobile Platform 2 Pose: [2.12287175096131;-6.96435795295058;2.80095969663273]
Robot Arm 2 end: [-6.29893769788921;2.50270493820851;0.683647683051698]
Robot Arm 1 togoal: 13.0658582511333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.002891e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-0.765726651516697;-4.04518316115527;-2.74958517667479]
Robot Arm 1 end: [-4.74159615806098;-2.7871657771487;0.147025962507415]
Robot Arm 1 togoal: 19.5630792707934
Mobile Platform 2 Pose: [1.68962721198387;-6.61776232176863;2.57737966773268]
Robot Arm 2 end: [-5.90095238176725;2.42197517971895;0.751200068622692]
Robot Arm 1 togoal: 13.1034442295315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.075703e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-0.665475808475994;-4.1041383424763;-2.75008040211771]
Robot Arm 1 end: [-4.80079425310017;-2.78844173777717;0.147475848356818]
Robot Arm 1 togoal: 19.6085267472666
Mobile Platform 2 Pose: [1.06448025544057;-6.1430255443293;2.56818458217977]
Robot Arm 2 end: [-5.38475100241342;2.46766234060719;0.66031660687629]
Robot Arm 1 togoal: 13.3223084881081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.825311e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-0.565875313731055;-4.16273047611396;-2.75056093116193]
Robot Arm 1 end: [-4.85961304654858;-2.7897295240929;0.147913050682447]
Robot Arm 1 togoal: 19.6537693321916
Mobile Platform 2 Pose: [0.714470723964329;-5.89311431296057;2.5615230192835]
Robot Arm 2 end: [-5.1161723017838;2.46586417840127;0.634413029505472]
Robot Arm 1 togoal: 13.4177337563563

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.770427e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-0.466919870549029;-4.22096230463903;-2.75102721971197]
Robot Arm 1 end: [-4.9180555062268;-2.79102792858234;0.148337309744647]
Robot Arm 1 togoal: 19.6988065226769
Mobile Platform 2 Pose: [0.323177375629243;-5.63873813257459;2.55299009979452]
Robot Arm 2 end: [-4.86387766929846;2.47074985616499;0.516825494070768]
Robot Arm 1 togoal: 13.524338098751

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.917801e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-0.368604366635436;-4.27883644835427;-2.75147971880717]
Robot Arm 1 end: [-4.97612450013827;-2.79233577342464;0.14874842385239]
Robot Arm 1 togoal: 19.7436378099863
Mobile Platform 2 Pose: [-0.0471277064778711;-5.38882493725032;2.54640262665743]
Robot Arm 2 end: [-4.64386620341453;2.44792780403836;0.371328488079255]
Robot Arm 1 togoal: 13.6000301634518
Step 249:
Mobile Platform 1 Pose: [-0.270923864111356;-4.33635541289445;-2.75191887384438]
Robot Arm 1 end: [-5.03382280334086;-2.79365191839107;0.14914624727487]
Robot Arm 1 togoal: 19.7882626880196
Mobile Platform 2 Pose: [-0.410564325895656;-5.13885197416085;2.54272598073469]
Robot Arm 2 end: [-4.41859530319786;2.43241624158084;0.29053901344722]
Robot Arm 1 togoal: 13.6832410719404
Step 250:
Mobile Platform 1 Pose: [-0.173873589781876;-4.39352159653491;-2.75234512397591]
Robot Arm 1 end: [-5.09115310387491;-2.79497526651593;0.149530687400796]
Robot Arm 1 togoal: 19.832680659728
Mobile Platform 2 Pose: [-0.770511923357749;-4.8889349742015;2.53628446412177]
Robot Arm 2 end: [-4.21694983904417;2.44107561943455;0.171034711205852]
Robot Arm 1 togoal: 13.7857331329737

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.662984e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-0.0774489254788116;-4.45033729736481;-2.75275890163451]
Robot Arm 1 end: [-5.14811800815568;-2.79630476808354;0.149901701428275]
Robot Arm 1 togoal: 19.8768912421192
Mobile Platform 2 Pose: [-1.13753912161212;-4.63898556310332;2.53112309547823]
Robot Arm 2 end: [-3.98013732145407;2.28948651781922;0.193614302953437]
Robot Arm 1 togoal: 13.7488973495671
Step 252:
Mobile Platform 1 Pose: [0.0183546015989718;-4.5068047203867;-2.75316063215262]
Robot Arm 1 end: [-5.20472004606247;-2.7976394233121;0.150259292783753]
Robot Arm 1 togoal: 19.9208939702681
Mobile Platform 2 Pose: [-1.47379249381915;-4.39158285916112;2.49518005577817]
Robot Arm 2 end: [-3.75334322213726;2.28073632047985;0.137051858386003]
Robot Arm 1 togoal: 13.8474733410918
Step 253:
Mobile Platform 1 Pose: [0.113541328546737;-4.56292598455106;-2.75355073345438]
Robot Arm 1 end: [-5.26096167585046;-2.79897828400933;0.150603507411288]
Robot Arm 1 togoal: 19.9646884006042
Mobile Platform 2 Pose: [-1.82666433769719;-4.14333213840645;2.46565752801804]
Robot Arm 2 end: [-3.46965002604645;2.23661008910983;0.192885928076307]
Robot Arm 1 togoal: 13.9335560516065
Step 254:
Mobile Platform 1 Pose: [0.208115466375338;-4.61870312970621;-2.75392961580472]
Robot Arm 1 end: [-5.31684528894792;-2.80032045440502;0.150934430033762]
Robot Arm 1 togoal: 20.0082741136519
Mobile Platform 2 Pose: [-2.18220322643018;-3.89502734547824;2.43659329207476]
Robot Arm 2 end: [-3.25898008792374;2.20971502584774;0.137987050651563]
Robot Arm 1 togoal: 14.0153920846149
Step 255:
Mobile Platform 1 Pose: [0.302081109533497;-4.67413812343104;-2.75429768160483]
Robot Arm 1 end: [-5.37237321466356;-2.80166509131898;0.151252180459393]
Robot Arm 1 togoal: 20.051650716346
Mobile Platform 2 Pose: [-2.53954634552127;-3.64671251992657;2.40761489647994]
Robot Arm 2 end: [-2.9830602112322;2.14774621191834;0.193179839114022]
Robot Arm 1 togoal: 14.0914260601269
Step 256:
Mobile Platform 1 Pose: [0.395442245265919;-4.72923286771294;-2.75465532522573]
Robot Arm 1 end: [-5.42754772480718;-2.80301140379031;0.151556909986405]
Robot Arm 1 togoal: 20.0948178440008
Mobile Platform 2 Pose: [-2.8906309443113;-3.39885632462778;2.37494524003748]
Robot Arm 2 end: [-2.77659681977046;2.11062610026871;0.1388568882554]
Robot Arm 1 togoal: 14.1688086170293
Step 257:
Mobile Platform 1 Pose: [0.488202762744001;-4.78398920543375;-2.75500293287391]
Robot Arm 1 end: [-5.48237103821443;-2.80435865227023;0.151848797943454]
Robot Arm 1 togoal: 20.1377751619935
Mobile Platform 2 Pose: [-3.24215804151148;-3.1512191325549;2.34065499211975]
Robot Arm 2 end: [-2.50828409105617;2.03391605186335;0.194178366497139]
Robot Arm 1 togoal: 14.2373241323905
Step 258:
Mobile Platform 1 Pose: [0.580366461911583;-4.83840892662958;-2.7553408824844]
Robot Arm 1 end: [-5.53684532516064;-2.80570614746397;0.15212804839191]
Robot Arm 1 togoal: 20.1805223672001
Mobile Platform 2 Pose: [-3.58341419955312;-2.9045207388153;2.30015928214445]
Robot Arm 2 end: [-2.30721061674283;1.98375559276221;0.140222132070317]
Robot Arm 1 togoal: 14.3071993808285
Step 259:
Mobile Platform 1 Pose: [0.671937061995731;-4.8924937744945;-2.75566954363738]
Robot Arm 1 end: [-5.59097271164594;-2.80705324889297;0.152394887007282]
Robot Arm 1 togoal: 20.2230591892171
Mobile Platform 2 Pose: [-3.92103259242983;-2.65857349295002;2.25532679112179]
Robot Arm 2 end: [-2.0488594079545;1.89114779530591;0.195365846623624]
Robot Arm 1 togoal: 14.3658392902357
Step 260:
Mobile Platform 1 Pose: [0.762918209640178;-4.94624545110278;-2.75598927749582]
Robot Arm 1 end: [-5.64475528353315;-2.80839936323899;0.152649558150299]
Robot Arm 1 togoal: 20.2653853913902
Mobile Platform 2 Pose: [-4.23982868984162;-2.41466436141681;2.20047849529853]
Robot Arm 2 end: [-1.85639168825307;1.82086534354633;0.140988106329211]
Robot Arm 1 togoal: 14.4206673435067
Step 261:
Mobile Platform 1 Pose: [0.853313486630637;-4.99966562283181;-2.75630043676086]
Robot Arm 1 end: [-5.69819509052356;-2.80974394252118;0.152892322132893]
Robot Arm 1 togoal: 20.307500771669
Mobile Platform 2 Pose: [-4.54447922153281;-2.17324228333296;2.13555253582173]
Robot Arm 2 end: [-1.61471955956043;1.70273578664663;0.196199986199398]
Robot Arm 1 togoal: 14.4575948218728
Step 262:
Mobile Platform 1 Pose: [0.943126417184461;-5.05275592546904;-2.75660336564383]
Robot Arm 1 end: [-5.75129414995394;-2.81108648215171;0.153123452680261]
Robot Arm 1 togoal: 20.3494051633027
Mobile Platform 2 Pose: [-4.80637938971607;-1.93782842128814;2.05140793672307]
Robot Arm 2 end: [-1.44134069281901;1.57772083049625;0.155335419129102]
Robot Arm 1 togoal: 14.4623410453753
Step 263:
Mobile Platform 1 Pose: [1.03236047478586;-5.10551796899202;-2.75689839985302]
Robot Arm 1 end: [-5.80405445040101;-2.81242651890776;0.153343234587038]
Robot Arm 1 togoal: 20.3910984353867
Mobile Platform 2 Pose: [-5.01462727628384;-1.71709838725087;1.94027067357565]
Robot Arm 2 end: [-1.27821518048003;1.38989321587225;0.1968690264965]
Robot Arm 1 togoal: 14.4066831971128

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','8.875326e-15','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [1.12101908856045;-5.15795334201925;-2.75718586659454]
Robot Arm 1 end: [-5.85647795508704;-2.81376362885328;0.153551961563238]
Robot Arm 1 togoal: 20.4325804932737
Mobile Platform 2 Pose: [-5.20858855134427;-1.50953311816834;1.80092700105804]
Robot Arm 2 end: [-1.10524783172675;1.25452835917433;0.139319802270388]
Robot Arm 1 togoal: 14.4114560295363

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.134146e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [1.20910564918021;-5.21006361592674;-2.75746608458515]
Robot Arm 1 end: [-5.90856660507583;-2.81509742523717;0.153749934263811]
Robot Arm 1 togoal: 20.4738512788559
Mobile Platform 2 Pose: [-5.13267927294186;-1.45110159475927;1.67306471050174]
Robot Arm 2 end: [-1.17622597277438;1.10790718596651;0.0844908431591421]
Robot Arm 1 togoal: 14.2586166472514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.822606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [1.29662351430055;-5.2618503486323;-2.7577393640765]
Robot Arm 1 end: [-5.96032232225436;-2.8164275563916;0.153937458494343]
Robot Arm 1 togoal: 20.5149107707266
Mobile Platform 2 Pose: [-4.77826576738978;-1.57165555896974;1.55956201139208]
Robot Arm 2 end: [-1.3822167236281;0.895905440047754;0.140374502820106]
Robot Arm 1 togoal: 13.9604108363773

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.535577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [1.38357601353562;-5.31331508805293;-2.75800600689032]
Robot Arm 1 end: [-6.01174701209847;-2.81775370365026;0.154114843584536]
Robot Arm 1 togoal: 20.5557589842291
Mobile Platform 2 Pose: [-4.56368591409767;-1.66682174232107;1.51512438566127]
Robot Arm 2 end: [-1.3683231581626;0.956730326659563;0.0900881104735343]
Robot Arm 1 togoal: 14.0215296379761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.204273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.342247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [1.46996645298091;-5.36445937524178;-2.75826630646355]
Robot Arm 1 end: [-6.06284256622151;-2.81907557930265;0.154282400920459]
Robot Arm 1 togoal: 20.5963959714006
Mobile Platform 2 Pose: [-4.34470563930814;-1.72353581473448;1.4188622095244]
Robot Arm 2 end: [-1.55629230564223;0.861120259157355;0.0466793648239929]
Robot Arm 1 togoal: 13.8359587397094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [1.55417808950227;-5.4144747323173;-2.7585205479032]
Robot Arm 1 end: [-6.11397124590655;-2.8053413215218;0.154440442619807]
Robot Arm 1 togoal: 20.627756075212
Mobile Platform 2 Pose: [-3.86408205644859;-1.93036113094701;1.27842258344389]
Robot Arm 2 end: [-1.85168992778416;0.577116543470189;0.169974583011287]
Robot Arm 1 togoal: 13.4199709792709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984607e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [1.63967719246621;-5.46516336898658;-2.75848111081647]
Robot Arm 1 end: [-6.15027751795058;-2.80337672185811;0.1444033134512]
Robot Arm 1 togoal: 20.6555788547805
Mobile Platform 2 Pose: [-3.70111613069683;-2.0123456437169;1.25222804697256]
Robot Arm 2 end: [-2.06969785199035;0.54267932784509;0.181272010975063]
Robot Arm 1 togoal: 13.2601904455809

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000006e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [1.73904516023363;-5.52505591145454;-2.75747339327282]
Robot Arm 1 end: [-6.20228675926761;-2.80035016415937;0.138381931447491]
Robot Arm 1 togoal: 20.6947987010776
Mobile Platform 2 Pose: [-3.56791114593225;-2.08354693657717;1.23520656760797]
Robot Arm 2 end: [-2.27187571822344;0.549318737723145;0.186103374486051]
Robot Arm 1 togoal: 13.14511238999

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001146e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [1.84556646195556;-5.58969696383728;-2.75589101207186]
Robot Arm 1 end: [-6.26201117154164;-2.80387992669477;0.134490660084576]
Robot Arm 1 togoal: 20.7440294222274
Mobile Platform 2 Pose: [-3.45020911592921;-2.1492989613661;1.2223357617475]
Robot Arm 2 end: [-2.46047452288779;0.582041564968063;0.194473115549867]
Robot Arm 1 togoal: 13.0607703324844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001861e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [1.95479872196631;-5.65609511849145;-2.75423588775188]
Robot Arm 1 end: [-6.32515753072485;-2.81019149625172;0.131893531879642]
Robot Arm 1 togoal: 20.7976294647586
Mobile Platform 2 Pose: [-3.34404123516967;-2.21098478965956;1.21233916643029]
Robot Arm 2 end: [-2.63376686104462;0.638922046158496;0.204098809695632]
Robot Arm 1 togoal: 13.0070089454381

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002240e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [2.055764915961;-5.71831196117826;-2.75276705811157]
Robot Arm 1 end: [-6.38506705530629;-2.81738036724635;0.130082622110064]
Robot Arm 1 togoal: 20.8492281161119
Mobile Platform 2 Pose: [-3.24672486549135;-2.26961686134552;1.20464340498328]
Robot Arm 2 end: [-2.78921385167885;0.718010877069089;0.213687215719846]
Robot Arm 1 togoal: 12.9838461464134

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002425e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [2.15430427794862;-5.77902066989237;-2.75158107815619]
Robot Arm 1 end: [-6.44375110460367;-2.82396322393548;0.1285949420624]
Robot Arm 1 togoal: 20.8995161428291
Mobile Platform 2 Pose: [-3.15455015902349;-2.32707825020143;1.19881192548928]
Robot Arm 2 end: [-2.92505343966505;0.815560281173702;0.222996010327696]
Robot Arm 1 togoal: 12.9890733843682

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002486e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [2.25003385915753;-5.83797968086228;-2.75064634878726]
Robot Arm 1 end: [-6.50090376237783;-2.83005707023195;0.127371916830565]
Robot Arm 1 togoal: 20.9483266308997
Mobile Platform 2 Pose: [-3.0649968159575;-2.38480105118676;1.19473377358827]
Robot Arm 2 end: [-3.04066247085291;0.927188351381059;0.23365655105366]
Robot Arm 1 togoal: 13.0189506976061

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002428e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [2.35306254607557;-5.90022272508597;-2.74992845499937]
Robot Arm 1 end: [-6.56150505908313;-2.83592009835232;0.126380694931915]
Robot Arm 1 togoal: 20.9997442269993
Mobile Platform 2 Pose: [-2.97623072855847;-2.44399221246099;1.19254175389285]
Robot Arm 2 end: [-3.13729581972923;1.04872951011034;0.24993788867321]
Robot Arm 1 togoal: 13.0685397556246

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002250e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [2.45348782496318;-5.96078790538441;-2.74939644740334]
Robot Arm 1 end: [-6.62057687644011;-2.84149551171697;0.125595036209423]
Robot Arm 1 togoal: 21.049815621427
Mobile Platform 2 Pose: [-2.88624756524312;-2.50621586855601;1.19261319626351]
Robot Arm 2 end: [-3.21844523930053;1.1774901329639;0.279718930042545]
Robot Arm 1 togoal: 13.1326520299407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001497e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [2.5557952311726;-6.02193035278317;-2.74902058038186]
Robot Arm 1 end: [-6.67906807587466;-2.84450492021299;0.123960058739679]
Robot Arm 1 togoal: 21.0979700237372
Mobile Platform 2 Pose: [-2.79103866352467;-2.5750096100789;1.19583598666877]
Robot Arm 2 end: [-3.28967679291739;1.31340764949046;0.336300129068558]
Robot Arm 1 togoal: 13.2070111672265

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001456e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [2.65585578097563;-6.08180459602843;-2.74869151660222]
Robot Arm 1 end: [-6.73654096583874;-2.8477001882841;0.122632041386242]
Robot Arm 1 togoal: 21.145463735206
Mobile Platform 2 Pose: [-2.68107635105481;-2.66050553962651;1.20524175104929]
Robot Arm 2 end: [-3.35826110898615;1.45891824572409;0.441876293508009]
Robot Arm 1 togoal: 13.2884916688279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001397e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [2.75346933788703;-6.14028824084674;-2.74841533631462]
Robot Arm 1 end: [-6.79283211111014;-2.85094334076117;0.121548597052088]
Robot Arm 1 togoal: 21.1920838826341
Mobile Platform 2 Pose: [-2.55405946935918;-2.78670519846473;1.23685545675383]
Robot Arm 2 end: [-3.44198638633127;1.6215160553001;0.572092692383566]
Robot Arm 1 togoal: 13.3778773749467

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001350e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [2.84845844578688;-6.19728041832634;-2.7481928912312]
Robot Arm 1 end: [-6.8478600560446;-2.85425163875083;0.120703474172431]
Robot Arm 1 togoal: 21.2377730475828
Mobile Platform 2 Pose: [-2.53627988019111;-2.89467256274669;1.30857261583841]
Robot Arm 2 end: [-3.46788565662896;1.80689496021398;0.570285119799612]
Robot Arm 1 togoal: 13.5268305666861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001273e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [2.94106065371711;-6.25288804729125;-2.74802479594249]
Robot Arm 1 end: [-6.90176774421024;-2.85785332070882;0.12012881497174]
Robot Arm 1 togoal: 21.2827767214039
Mobile Platform 2 Pose: [-2.57169231167871;-2.97312666456374;1.39704402401768]
Robot Arm 2 end: [-3.43890452740275;1.98489220872323;0.502590474795028]
Robot Arm 1 togoal: 13.7011960432759

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001125e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [3.03199913163113;-6.30745741275363;-2.74791419815048]
Robot Arm 1 end: [-6.95483200765594;-2.86170363434378;0.119768911569495]
Robot Arm 1 togoal: 21.327287622872
Mobile Platform 2 Pose: [-2.52065712919132;-3.08957539081891;1.49429047137562]
Robot Arm 2 end: [-3.44308739564353;2.17132017424306;0.621445708245803]
Robot Arm 1 togoal: 13.8543616095769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000954e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [3.12117932050947;-6.36093747403229;-2.7478596293396]
Robot Arm 1 end: [-7.00693763008733;-2.86561669454222;0.119557898634435]
Robot Arm 1 togoal: 21.3711033980009
Mobile Platform 2 Pose: [-2.68962605913853;-3.06360042097554;1.58346456600102]
Robot Arm 2 end: [-3.4072173069884;2.24140174854594;0.414067000414158]
Robot Arm 1 togoal: 13.9478314110886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000681e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [3.20860238519284;-6.41333590974067;-2.74785500799746]
Robot Arm 1 end: [-7.05807791995951;-2.86975278068943;0.119512550221829]
Robot Arm 1 togoal: 21.4143112434502
Mobile Platform 2 Pose: [-2.58446239049119;-3.15216513215646;1.62551360941385]
Robot Arm 2 end: [-3.36254306263856;2.35973502155253;0.621445708341562]
Robot Arm 1 togoal: 14.0580255484887

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000489e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [3.29427246341998;-6.46466491974964;-2.74789454672771]
Robot Arm 1 end: [-7.10813560642056;-2.87355999656202;0.119468616844653]
Robot Arm 1 togoal: 21.4564941481764
Mobile Platform 2 Pose: [-2.77751002374013;-3.09913447677654;1.71432270098368]
Robot Arm 2 end: [-3.31804464928147;2.42546695371044;0.413064777559731]
Robot Arm 1 togoal: 14.1516174617557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000233e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.673200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [3.3783521755434;-6.51502810756721;-2.74796723501216]
Robot Arm 1 end: [-7.15739329779962;-2.87767742698772;0.119640635269039]
Robot Arm 1 togoal: 21.4982447831921
Mobile Platform 2 Pose: [-2.67916235842503;-3.17740683585455;1.75534410789688]
Robot Arm 2 end: [-3.25039384886566;2.51450768497604;0.621938054648119]
Robot Arm 1 togoal: 14.2470487531395

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000071e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.091400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [3.4608210162098;-6.56442181689543;-2.74806876597875]
Robot Arm 1 end: [-7.20568695170343;-2.8814765960961;0.119819246481404]
Robot Arm 1 togoal: 21.5390670707861
Mobile Platform 2 Pose: [-2.92701716402961;-3.11054131357768;1.8894690144249]
Robot Arm 2 end: [-3.32759508889797;2.60118206785552;0.413064777550749]
Robot Arm 1 togoal: 14.3016886157574

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989070e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [3.52601577267391;-6.60503167478548;-2.74819100831877]
Robot Arm 1 end: [-7.24366487073085;-2.88501157241756;0.119135306373856]
Robot Arm 1 togoal: 21.571572172993
Mobile Platform 2 Pose: [-2.83251331721118;-3.18590324358916;1.92914431486251]
Robot Arm 2 end: [-3.25713337253777;2.68958249391649;0.621445708535878]
Robot Arm 1 togoal: 14.3979495814143

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988930e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [3.58910914834564;-6.64461426290965;-2.74829521994662]
Robot Arm 1 end: [-7.28097204324445;-2.88875802563324;0.118892677271489]
Robot Arm 1 togoal: 21.6036567077699
Mobile Platform 2 Pose: [-3.03854734113979;-3.11286191913821;2.01404031602888]
Robot Arm 2 end: [-3.2008946695818;2.75336922755286;0.412810704189157]
Robot Arm 1 togoal: 14.4949764906247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [3.65623690530414;-6.68620595488383;-2.74838831540222]
Robot Arm 1 end: [-7.32296365397248;-2.89400353457162;0.12065111064738]
Robot Arm 1 togoal: 21.6402720389164
Mobile Platform 2 Pose: [-2.94556859822357;-3.18234639832336;2.05319509648951]
Robot Arm 2 end: [-3.11813302705723;2.81425790600906;0.621445707999111]
Robot Arm 1 togoal: 14.573074371568

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995838e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [3.72059014638234;-6.72619776084203;-2.74850796069038]
Robot Arm 1 end: [-7.3630018795059;-2.89832583094777;0.122002807089418]
Robot Arm 1 togoal: 21.6748203373129
Mobile Platform 2 Pose: [-3.17009259399798;-3.08876370002259;2.13597334402566]
Robot Arm 2 end: [-3.05280285622296;2.88025723526672;0.412495843269361]
Robot Arm 1 togoal: 14.6762854996295

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [3.79351053191997;-6.77031026310221;-2.74863975762545]
Robot Arm 1 end: [-7.41764614366155;-2.88960193680092;0.12783898514104]
Robot Arm 1 togoal: 21.7130636458403
Mobile Platform 2 Pose: [-3.07538798295583;-3.15556500869098;2.17631806338921]
Robot Arm 2 end: [-2.96587459193524;2.91615419579308;0.621445707737409]
Robot Arm 1 togoal: 14.7348280407548

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [3.86066972037896;-6.81084720922597;-2.74862514543706]
Robot Arm 1 end: [-7.46342537588754;-2.89064741786025;0.131556331851841]
Robot Arm 1 togoal: 21.7501855260533
Mobile Platform 2 Pose: [-3.31846072757829;-3.04226701776849;2.25553884202571]
Robot Arm 2 end: [-2.89237148283583;2.98560959974706;0.41279127241066]
Robot Arm 1 togoal: 14.8449508722311

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.410065e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [3.71272496276938;-6.74339743491564;-2.74865948917803]
Robot Arm 1 end: [-7.39540012922405;-2.91027051067983;0.134113885324598]
Robot Arm 1 togoal: 21.7071067489386
Mobile Platform 2 Pose: [-3.21654306699104;-3.11014542538546;2.29840877964967]
Robot Arm 2 end: [-2.80914152742161;3.00039592685732;0.621755178477582]
Robot Arm 1 togoal: 14.8837959229739

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.188474e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [3.01010289645215;-6.49339756532459;-2.74891976514878]
Robot Arm 1 end: [-7.15042127602154;-2.90374843337595;0.135998001757373]
Robot Arm 1 togoal: 21.5072523608201
Mobile Platform 2 Pose: [-3.47674139892245;-2.97881539245594;2.3720476541783]
Robot Arm 2 end: [-2.72808045858726;3.07440781163821;0.413741763945566]
Robot Arm 1 togoal: 15.0015657263873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.677097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [3.08028103901552;-6.53511241660519;-2.74931054082335]
Robot Arm 1 end: [-7.19627713405172;-2.90568599182674;0.139928722540498]
Robot Arm 1 togoal: 21.5447449756122
Mobile Platform 2 Pose: [-3.36317389225443;-3.05102130898348;2.42106652856604]
Robot Arm 2 end: [-2.6548570454984;3.07421202314579;0.622750122298087]
Robot Arm 1 togoal: 15.0230821701482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.265832e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [3.14891354919552;-6.5758368022724;-2.74974114819421]
Robot Arm 1 end: [-7.24259576923265;-2.9074346988199;0.146170258111865]
Robot Arm 1 togoal: 21.5823802251892
Mobile Platform 2 Pose: [-3.63637267778338;-2.90511378070558;2.48694951833569]
Robot Arm 2 end: [-2.5664103541762;3.1521639973306;0.415170437249225]
Robot Arm 1 togoal: 15.1479547087068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.173978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [3.21615462823502;-6.61567000024633;-2.75020051279706]
Robot Arm 1 end: [-7.28936236315504;-2.90904704600388;0.154717297207068]
Robot Arm 1 togoal: 21.6201821219058
Mobile Platform 2 Pose: [-3.50843469326465;-2.98331430532907;2.54716667087193]
Robot Arm 2 end: [-2.50706804628156;3.14462850425506;0.624243541553926]
Robot Arm 1 togoal: 15.1568288214816
Step 301:
Mobile Platform 1 Pose: [3.28215903102151;-6.65470917507615;-2.75067981346419]
Robot Arm 1 end: [-7.3365383604428;-2.91055611892923;0.165570161105396]
Robot Arm 1 togoal: 21.6581442342434
Mobile Platform 2 Pose: [-3.7895711665489;-2.82705706036431;2.60440578701312]
Robot Arm 2 end: [-2.41042994364386;3.22504537775715;0.417287683497971]
Robot Arm 1 togoal: 15.2879553819885

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.386815e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [3.34708143565622;-6.69304960944981;-2.75117189509236]
Robot Arm 1 end: [-7.38406482086596;-2.9119823045561;0.178734945200829]
Robot Arm 1 togoal: 21.6962363604053
Mobile Platform 2 Pose: [-3.65062655652842;-2.90865802887356;2.6844798704997]
Robot Arm 2 end: [-2.36502089494837;3.22001069883278;0.595991151655834]
Robot Arm 1 togoal: 15.2943528853411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.940216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [3.41107609538527;-6.73078495171048;-2.75167084233823]
Robot Arm 1 end: [-7.43186492052045;-2.91333806178044;0.194223235514909]
Robot Arm 1 togoal: 21.7344093797802
Mobile Platform 2 Pose: [-4.04628794509454;-2.67983707135669;2.78530153855169]
Robot Arm 2 end: [-2.26610443912524;3.39647493981401;0.387681127212858]
Robot Arm 1 togoal: 15.5100964867082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.872088e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [3.47429659402021;-6.76800741548727;-2.75217166367059]
Robot Arm 1 end: [-7.47984575875126;-2.91463118474464;0.212051467248163]
Robot Arm 1 togoal: 21.772598665504
Mobile Platform 2 Pose: [-3.91457420961589;-2.754994900763;2.82194477010979]
Robot Arm 2 end: [-2.20880530357948;3.35487208729855;0.595991151655834]
Robot Arm 1 togoal: 15.4890595142233

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000463e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [3.53689567791556;-6.80480794490482;-2.75267005473077]
Robot Arm 1 end: [-7.52789960874953;-2.91586683575889;0.232240016577553]
Robot Arm 1 togoal: 21.810726331517
Mobile Platform 2 Pose: [-4.3127136214434;-2.5248665496447;2.91962251595447]
Robot Arm 2 end: [-2.10819221822342;3.5287941821688;0.387681127212858]
Robot Arm 1 togoal: 15.7032899022915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.939581e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [3.59902519539086;-6.8412763840418;-2.7531622217745]
Robot Arm 1 end: [-7.5759047289724;-2.91704848989459;0.254812097452041]
Robot Arm 1 togoal: 21.8487024909206
Mobile Platform 2 Pose: [-4.12178539392654;-2.63488400699779;2.96618418959651]
Robot Arm 2 end: [-2.08650361825122;3.49524151728428;0.596527903102658]
Robot Arm 1 togoal: 15.6716182078216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993508e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [3.66083619104547;-6.87750169269593;-2.75364475231167]
Robot Arm 1 end: [-7.62372583540004;-2.91817789963748;0.279792543880597]
Robot Arm 1 togoal: 21.8864256679147
Mobile Platform 2 Pose: [-4.51787523504492;-2.41176455911616;3.07895725678778]
Robot Arm 2 end: [-1.99238420205423;3.68493965125635;0.388296108814184]
Robot Arm 1 togoal: 15.8959901274235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955173e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [3.72247919923583;-6.91357224394415;-2.75411452314382]
Robot Arm 1 end: [-7.67121430870948;-2.91925416706186;0.307206539923207]
Robot Arm 1 togoal: 21.9237824734057
Mobile Platform 2 Pose: [-4.37448187060266;-2.49369949409036;3.11210627318208]
Robot Arm 2 end: [-1.94299109952979;3.63873989431017;0.596527903102657]
Robot Arm 1 togoal: 15.8677429707204

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.395749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [3.78410477335696;-6.94957623122921;-2.75456863676065]
Robot Arm 1 end: [-7.71820817927694;-2.92027203146399;0.337078350698557]
Robot Arm 1 togoal: 21.9606466417296
Mobile Platform 2 Pose: [-4.7646877118873;-2.26836656554818;3.2203888530238]
Robot Arm 2 end: [-1.84637972308897;3.82455907000453;0.388296108814184]
Robot Arm 1 togoal: 16.0898407451785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.024038e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [3.84586428226894;-6.98560220753421;-2.75500437647128]
Robot Arm 1 end: [-7.7645318979276;-2.92121951816603;0.369430091855947]
Robot Arm 1 togoal: 21.996877520286
Mobile Platform 2 Pose: [-4.64675467692719;-2.33554776289959;3.24963258100774]
Robot Arm 2 end: [-1.78013269916831;3.77129992111002;0.596527903100406]
Robot Arm 1 togoal: 16.0645438347434

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.392713e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [3.90791100797736;-7.02173977988816;-2.75541916934705]
Robot Arm 1 end: [-7.80999586509357;-2.92207514858114;0.404280564773247]
Robot Arm 1 togoal: 22.0323181067549
Mobile Platform 2 Pose: [-5.03788497402051;-2.10779514993028;3.35272840433399]
Robot Arm 2 end: [-1.68039052750258;3.9530781649147;0.38829610881161]
Robot Arm 1 togoal: 16.2845727655411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.526559e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [3.97040159587665;-7.05808049252661;-2.75581054458066]
Robot Arm 1 end: [-7.85439565240245;-2.92280492841571;0.441644160721115]
Robot Arm 1 togoal: 22.0667927066486
Mobile Platform 2 Pose: [-4.92238325359897;-2.17359185139069;3.38152358390062]
Robot Arm 2 end: [-1.61415249411065;3.89887289749358;0.596527903100406]
Robot Arm 1 togoal: 16.2590207229912

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.177039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [4.03349794643229;-7.09471895488456;-2.75617607320991]
Robot Arm 1 end: [-7.8975108157478;-2.92335937416132;0.481529849066495]
Robot Arm 1 togoal: 22.1001042782204
Mobile Platform 2 Pose: [-5.31337730382073;-1.94492273774859;3.4825702922726]
Robot Arm 2 end: [-1.51288190836907;4.07959229190991;0.388296108811611]
Robot Arm 1 togoal: 16.4787472434288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [4.09736970756528;-7.13175431304011;-2.75651327358975]
Robot Arm 1 end: [-7.93910315229054;-2.92367079380857;0.523940269905434]
Robot Arm 1 togoal: 22.132031465743
Mobile Platform 2 Pose: [-5.2001262097643;-2.00943263017907;3.51087564393696]
Robot Arm 2 end: [-1.44749198988366;4.02550699114062;0.596527903103199]
Robot Arm 1 togoal: 16.4534157025093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [4.16219764520473;-7.16929223396161;-2.75681946444789]
Robot Arm 1 end: [-7.97891418914075;-2.92365097731632;0.568871000802099]
Robot Arm 1 togoal: 22.162325230785
Mobile Platform 2 Pose: [-5.58969943028625;-1.78213050065072;3.60943397226044]
Robot Arm 2 end: [-1.34232464300638;4.2007590553369;0.388296108814803]
Robot Arm 1 togoal: 16.6703304838247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [4.22817836502783;-7.20744768792402;-2.75709154264782]
Robot Arm 1 end: [-8.01666157858484;-2.92318932641024;0.616310157713703]
Robot Arm 1 togoal: 22.1907048223853
Mobile Platform 2 Pose: [-5.39066675020975;-1.89494883692232;3.64750616619831]
Robot Arm 2 end: [-1.32637297294847;4.15648169447437;0.595991151655835]
Robot Arm 1 togoal: 16.6280913258227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [4.29553118527375;-7.24634900748314;-2.75732565303517]
Robot Arm 1 end: [-8.0520338634672;-2.92215126230685;0.666238650054095]
Robot Arm 1 togoal: 22.2168525407868
Mobile Platform 2 Pose: [-5.66338635351801;-1.73706282196077;3.7109833674137]
Robot Arm 2 end: [-1.20912867886942;4.22672861260954;0.387681127212858]
Robot Arm 1 togoal: 16.7619699698136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [4.36450852691858;-7.28614403540033;-2.75751669757051]
Robot Arm 1 end: [-8.0846826676502;-2.92037646516229;0.718631679345411]
Robot Arm 1 togoal: 22.2404062488327
Mobile Platform 2 Pose: [-5.48429392893993;-1.83577426181113;3.76379129662435]
Robot Arm 2 end: [-1.19418574520053;4.17694838155806;0.595991151655834]
Robot Arm 1 togoal: 16.7147800306467

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968178e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [4.43541222221899;-7.32700978458005;-2.7576575891443]
Robot Arm 1 end: [-8.11421055856235;-2.91767605109179;0.773462530779552]
Robot Arm 1 togoal: 22.2609476660085
Mobile Platform 2 Pose: [-5.77875149928299;-1.65915495721291;3.81663924071717]
Robot Arm 2 end: [-1.13018343317799;4.33132044399207;0.387681123183134]
Robot Arm 1 togoal: 16.892152120852

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.850870e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [4.50862015992253;-7.36916822585976;-2.75773806656516]
Robot Arm 1 end: [-8.1401511800773;-2.91382701052705;0.830710533144408]
Robot Arm 1 togoal: 22.2779826834013
Mobile Platform 2 Pose: [-5.63782369665245;-1.73662704868177;3.86120900681339]
Robot Arm 2 end: [-1.09428556462077;4.2657480843255;0.559821820358026]
Robot Arm 1 togoal: 16.8447968673706

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004335e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [4.58463193031838;-7.41291333480038;-2.75774268753147]
Robot Arm 1 end: [-8.16193471138291;-2.90856068775732;0.890376670373599]
Robot Arm 1 togoal: 22.2909061564109
Mobile Platform 2 Pose: [-5.9751549136316;-1.52589777655865;3.9964176103383]
Robot Arm 2 end: [-1.00191421136113;4.49615767356712;0.35305542539187]
Robot Arm 1 togoal: 17.1017025376417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.886522e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [4.66415190911657;-7.45866033389198;-2.75764712858512]
Robot Arm 1 end: [-8.17882348992325;-2.90153863052845;0.952513779932336]
Robot Arm 1 togoal: 22.2989349318574
Mobile Platform 2 Pose: [-5.8892301957994;-1.57397004506121;4.00957679584707]
Robot Arm 2 end: [-0.937218179221705;4.41046696131671;0.513468394811788]
Robot Arm 1 togoal: 17.0531724998513

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.097296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [4.74825342318329;-7.50704306828842;-2.75741058137173]
Robot Arm 1 end: [-8.18978142622302;-2.89230034666422;1.01728660568748]
Robot Arm 1 togoal: 22.3009704664612
Mobile Platform 2 Pose: [-5.57023528309737;-1.72796659143995;4.20742190954966]
Robot Arm 2 end: [-1.00651197603077;4.37240974725061;0.64506325923238]
Robot Arm 1 togoal: 16.9769329960971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.721852e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [4.83874289722566;-7.55913036902406;-2.75695771531857]
Robot Arm 1 end: [-8.19317741071417;-2.88014130834811;1.08509997124378]
Robot Arm 1 togoal: 22.2952854490766
Mobile Platform 2 Pose: [-5.41162822944858;-1.7518661650694;4.505571346948]
Robot Arm 2 end: [-1.02798414014724;4.57121146023817;0.58111914162287]
Robot Arm 1 togoal: 17.1376966957748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.092233e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [4.93911933897157;-7.61699788201871;-2.75612612997021]
Robot Arm 1 end: [-8.18598919897803;-2.86378292924394;1.15690859308134]
Robot Arm 1 togoal: 22.2786821187575
Mobile Platform 2 Pose: [-5.23028251387367;-1.87412013842776;4.4937121263761]
Robot Arm 2 end: [-1.18722307078807;4.57462922059462;0.376621512722449]
Robot Arm 1 togoal: 17.0702799111205
Step 326:
Mobile Platform 1 Pose: [5.0579276678366;-7.6857376374309;-2.75445200318024]
Robot Arm 1 end: [-8.16098230237125;-2.84019942668384;1.23512775788671]
Robot Arm 1 togoal: 22.2435096700783
Mobile Platform 2 Pose: [-5.20350934634274;-1.89337317176014;4.48815904729254]
Robot Arm 2 end: [-1.15393835626007;4.57666190160056;0.482367929572405]
Robot Arm 1 togoal: 17.082465728815
Step 327:
Mobile Platform 1 Pose: [5.22963349451578;-7.78613810639842;-2.74936809957353]
Robot Arm 1 end: [-8.13658106416607;-2.80543530263517;1.31573526177802]
Robot Arm 1 togoal: 22.2026322673642
Mobile Platform 2 Pose: [-4.76297295329531;-2.05595847218947;4.67806960540953]
Robot Arm 2 end: [-1.2988746082224;4.77130949125314;0.573633574384592]
Robot Arm 1 togoal: 17.1697021032459
Step 328:
Mobile Platform 1 Pose: [4.77104197702381;-7.55213317646198;-2.67831716563764]
Robot Arm 1 end: [-8.04707748715968;-2.82530086818885;1.22419142554816]
Robot Arm 1 togoal: 22.1421192191935
Mobile Platform 2 Pose: [-5.1307102910737;-1.81364942138251;4.71512917310332]
Robot Arm 2 end: [-1.08745552163527;4.8511859214477;0.377948357599881]
Robot Arm 1 togoal: 17.3579301591356
Step 329:
Mobile Platform 1 Pose: [4.68089843850612;-7.50308083259603;-2.66939263582313]
Robot Arm 1 end: [-8.12146629561395;-2.81138993519112;1.12231964079358]
Robot Arm 1 togoal: 22.1963555297577
Mobile Platform 2 Pose: [-5.35455173612158;-1.61272328161385;4.86388631194991]
Robot Arm 2 end: [-0.907004993265048;4.99733926554299;0.295675009062799]
Robot Arm 1 togoal: 17.5813584393163
Step 330:
Mobile Platform 1 Pose: [4.56575032976032;-7.44180100645029;-2.65644374938297]
Robot Arm 1 end: [-8.14878315907795;-2.81831446647636;1.02035450856773]
Robot Arm 1 togoal: 22.2247196118376
Mobile Platform 2 Pose: [-5.67881362422643;-1.37778715211575;4.94935569276734]
Robot Arm 2 end: [-0.619953849899288;5.03678603058392;0.352992002311427]
Robot Arm 1 togoal: 17.7610372047988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.155769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [4.46217368681532;-7.38654187673619;-2.64567713399605]
Robot Arm 1 end: [-8.15996325555381;-2.83633178738101;0.893403618981422]
Robot Arm 1 togoal: 22.2474617971637
Mobile Platform 2 Pose: [-5.860499422243;-1.26624612217125;4.95328015239096]
Robot Arm 2 end: [-0.551018832335689;5.03402135775708;0.250261790947321]
Robot Arm 1 togoal: 17.8022419624135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.595405e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [4.33929603404657;-7.32089869513922;-2.63160193106017]
Robot Arm 1 end: [-8.13689767552214;-2.84322651750506;0.758356531783297]
Robot Arm 1 togoal: 22.2368421708641
Mobile Platform 2 Pose: [-6.09791043558715;-1.1166459004144;4.96414186940923]
Robot Arm 2 end: [-0.445296454385933;5.03692867539153;0.163815914405314]
Robot Arm 1 togoal: 17.8673873992695

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.782791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [4.18939197537644;-7.24001498214033;-2.61314245852083]
Robot Arm 1 end: [-8.0725039297744;-2.84468135823905;0.628489727040553]
Robot Arm 1 togoal: 22.1900385775632
Mobile Platform 2 Pose: [-6.37968423980123;-0.935248924061719;4.98278121655877]
Robot Arm 2 end: [-0.253277749529263;5.05189434321249;0.159636612474108]
Robot Arm 1 togoal: 17.9836433522229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004988e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [4.01466451776052;-7.14323737789716;-2.59029518970144]
Robot Arm 1 end: [-7.97041181878786;-2.83223943462514;0.503206860614066]
Robot Arm 1 togoal: 22.1051571520982
Mobile Platform 2 Pose: [-6.56910631268384;-0.818306407653765;4.98660196325875]
Robot Arm 2 end: [-0.132118945577076;5.05303144077045;0.159955050940326]
Robot Arm 1 togoal: 18.0505200870896

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004519e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [3.79790023094493;-7.01755831460807;-2.55789715645604]
Robot Arm 1 end: [-7.82289085485148;-2.80220911706178;0.395822278627077]
Robot Arm 1 togoal: 21.9731204339666
Mobile Platform 2 Pose: [-6.74179489604122;-0.712489008112715;4.98860709150125]
Robot Arm 2 end: [-0.0235999275609092;5.05266614301137;0.160255629611537]
Robot Arm 1 togoal: 18.1097471451113

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004004e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [3.51899924547561;-6.84072928702685;-2.50919766086185]
Robot Arm 1 end: [-7.6182040128724;-2.73643695214202;0.312645204138089]
Robot Arm 1 togoal: 21.7733336948609
Mobile Platform 2 Pose: [-6.90516650468609;-0.612777255372607;4.98971658736612]
Robot Arm 2 end: [0.078152515089863;5.05155716652985;0.160674462434867]
Robot Arm 1 togoal: 18.1650493206839

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002274e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.638760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [3.21256159231971;-6.59985853798405;-2.44225528068891]
Robot Arm 1 end: [-7.39126711232336;-2.66423958091538;0.372114178506873]
Robot Arm 1 togoal: 21.544351337783
Mobile Platform 2 Pose: [-7.06219054646946;-0.51718179238652;4.99027060544048]
Robot Arm 2 end: [0.180349673487532;5.0489877265382;0.165948035496359]
Robot Arm 1 togoal: 18.2194323977033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002325e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880798e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [3.09247619077767;-6.42810225177354;-2.4230831440931]
Robot Arm 1 end: [-7.22663429611388;-2.63491391578955;0.455833780577276]
Robot Arm 1 togoal: 21.3900107037481
Mobile Platform 2 Pose: [-7.21903262485228;-0.419546594364253;4.99066161929051]
Robot Arm 2 end: [0.284022251585114;5.04642715527985;0.170938263800684]
Robot Arm 1 togoal: 18.2750855128448

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.408717e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.927015e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [2.94878634069836;-6.32609098428598;-2.41602467334346]
Robot Arm 1 end: [-7.10092577193757;-2.62546787572488;0.471818173856878]
Robot Arm 1 togoal: 21.2825018229199
Mobile Platform 2 Pose: [-7.38181783669605;-0.319136126269309;4.99048585827533]
Robot Arm 2 end: [0.389363985488557;5.04466971626508;0.175473623889451]
Robot Arm 1 togoal: 18.3327960803386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949067e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [2.84522074086663;-6.25876459876172;-2.41423263679081]
Robot Arm 1 end: [-7.00212647137309;-2.67689818595621;0.616054707396541]
Robot Arm 1 togoal: 21.227222572637
Mobile Platform 2 Pose: [-7.54805102355738;-0.21603843790973;4.99008232071557]
Robot Arm 2 end: [0.496607213516377;5.04385605235087;0.180110608260178]
Robot Arm 1 togoal: 18.3927798953714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961007e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [2.81353380017596;-6.23499789723707;-2.4148568214859]
Robot Arm 1 end: [-6.93665024320308;-2.67452508772646;0.687301529046705]
Robot Arm 1 togoal: 21.1704690736995
Mobile Platform 2 Pose: [-7.71669733911969;-0.111196666914466;4.98940086716047]
Robot Arm 2 end: [0.604429776152814;5.04482424631659;0.184828881315182]
Robot Arm 1 togoal: 18.4549727915898

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [2.81463641130249;-6.22691990507645;-2.41684418082001]
Robot Arm 1 end: [-6.8677512490936;-2.6771459012183;0.750283176114092]
Robot Arm 1 togoal: 21.1145699654219
Mobile Platform 2 Pose: [-7.89630955377011;8.71289751113169e-05;4.98861068227465]
Robot Arm 2 end: [0.7024853552077;5.05969390955667;0.171102035068606]
Robot Arm 1 togoal: 18.5245728140678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992495e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [2.85375214786691;-6.23305970218043;-2.41950746610543]
Robot Arm 1 end: [-6.80284411339374;-2.67394752233119;0.799475859241995]
Robot Arm 1 togoal: 21.0590862380948
Mobile Platform 2 Pose: [-8.08531886951343;0.117468734508641;4.98927675453368]
Robot Arm 2 end: [0.813392072878217;5.0648188520452;0.166789129758697]
Robot Arm 1 togoal: 18.5933348877441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [2.89622353686943;-6.23385224860364;-2.41861090011544]
Robot Arm 1 end: [-6.73122369863653;-2.68024340031602;0.831193959133347]
Robot Arm 1 togoal: 21.0047197438835
Mobile Platform 2 Pose: [-8.31042102811711;0.260782996427904;4.99181195296802]
Robot Arm 2 end: [0.945712386017724;5.12986032585456;0.169211993049054]
Robot Arm 1 togoal: 18.7229437838452

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [2.9251449176671;-6.22741013830789;-2.41097319305328]
Robot Arm 1 end: [-6.6518794415625;-2.69219722983817;0.850132700592128]
Robot Arm 1 togoal: 20.9481985476283
Mobile Platform 2 Pose: [-8.52001821535833;0.391867997511064;4.99763899497113]
Robot Arm 2 end: [1.06539242609743;5.17644290490316;0.168352745919625]
Robot Arm 1 togoal: 18.8307456937661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.200127e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989459e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [2.97624698283196;-6.23335729498376;-2.40037622112777]
Robot Arm 1 end: [-6.63995358358293;-2.67673591309184;0.773445519529977]
Robot Arm 1 togoal: 20.9319498942487
Mobile Platform 2 Pose: [-8.73285689633366;0.525103958620978;5.00621346104861]
Robot Arm 2 end: [1.18279494653041;5.23834771206028;0.169605706821309]
Robot Arm 1 togoal: 18.9496282376577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.814879e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989572e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [3.03559827967862;-6.24614825050979;-2.39563573487516]
Robot Arm 1 end: [-6.59676001036481;-2.685303525221;0.699791430498519]
Robot Arm 1 togoal: 20.9055914023797
Mobile Platform 2 Pose: [-8.93753850559861;0.65187251425418;5.01722976913048]
Robot Arm 2 end: [1.28768258625367;5.30413086139248;0.168416574033655]
Robot Arm 1 togoal: 19.0645661244844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.087426e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [3.08264495450028;-6.25690419850641;-2.39260619362231]
Robot Arm 1 end: [-6.46082787005227;-2.77978661980889;0.720802044610398]
Robot Arm 1 togoal: 20.854786428615
Mobile Platform 2 Pose: [-9.12949348121431;0.768861657433628;5.03016587515268]
Robot Arm 2 end: [1.38025507917057;5.36678989594439;0.166183558436557]
Robot Arm 1 togoal: 19.1698712818688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.755589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989652e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [3.26857395753058;-6.32594174485437;-2.38432761558011]
Robot Arm 1 end: [-6.4036012065035;-2.82200763706758;0.712736282362388]
Robot Arm 1 togoal: 20.8359137698415
Mobile Platform 2 Pose: [-9.30748233078261;0.875144479734607;5.04421678478772]
Robot Arm 2 end: [1.46377833432682;5.41946118179026;0.162777554903825]
Robot Arm 1 togoal: 19.2619512263343

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972062e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990099e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [3.53378918251507;-6.42961036620313;-2.3613781236169]
Robot Arm 1 end: [-6.37491868612283;-2.79626424963038;0.628282002047808]
Robot Arm 1 togoal: 20.8009443571048
Mobile Platform 2 Pose: [-9.46764414756477;0.969251382126702;5.05889176172811]
Robot Arm 2 end: [1.54257933097242;5.45672450093951;0.158832091148432]
Robot Arm 1 togoal: 19.3390013010404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.887882e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [3.67866952178661;-6.47108152545383;-2.33551041233998]
Robot Arm 1 end: [-6.3623418384477;-2.79192712932906;0.633805388790587]
Robot Arm 1 togoal: 20.788139512675
Mobile Platform 2 Pose: [-9.61035646749795;1.05209551663586;5.0738929415932]
Robot Arm 2 end: [1.62483401026275;5.46981919136071;0.153364780324683]
Robot Arm 1 togoal: 19.3990334211179

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.938819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [3.70138036847204;-6.45982575223738;-2.3243838648293]
Robot Arm 1 end: [-6.27387156021488;-2.75761920293995;0.597653076128463]
Robot Arm 1 togoal: 20.6989930389721
Mobile Platform 2 Pose: [-9.71197625573147;1.11183318778619;5.08836312092282]
Robot Arm 2 end: [1.70852643317956;5.44114913224458;0.150294630086116]
Robot Arm 1 togoal: 19.4267215703472

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995102e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [3.66693597858159;-6.42605620214082;-2.31910224509736]
Robot Arm 1 end: [-6.16898256858818;-2.70966856636945;0.546693240237761]
Robot Arm 1 togoal: 20.5893405924616
Mobile Platform 2 Pose: [-9.81675440668484;1.17394693990664;5.10127576868832]
Robot Arm 2 end: [1.78730924526804;5.40723752537217;0.142490396406046]
Robot Arm 1 togoal: 19.4479797442824

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973240e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998052e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [3.62134737044811;-6.39008093680246;-2.31452371615298]
Robot Arm 1 end: [-6.07698007150591;-2.70037008931696;0.484673837637187]
Robot Arm 1 togoal: 20.5144500423485
Mobile Platform 2 Pose: [-9.92174541467777;1.23775508602356;5.11480117220597]
Robot Arm 2 end: [1.80938867564784;5.33624966248216;0.060927386455948]
Robot Arm 1 togoal: 19.4111830303906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001340e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [3.5862903887757;-6.36087025972103;-2.31058269688987]
Robot Arm 1 end: [-5.98537564403581;-2.6727986614147;0.429458578996354]
Robot Arm 1 togoal: 20.4284978172027
Mobile Platform 2 Pose: [-10.045752098186;1.32017814402527;5.1433805634888]
Robot Arm 2 end: [1.96170707832047;5.27346333817211;0.111209220332184]
Robot Arm 1 togoal: 19.4511857893668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004237e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [3.56511018572592;-6.33689078691852;-2.30507302519902]
Robot Arm 1 end: [-5.90154434460447;-2.64115910281929;0.390520911746274]
Robot Arm 1 togoal: 20.3454171492057
Mobile Platform 2 Pose: [-10.1642273442164;1.39638307353808;5.15180627551393]
Robot Arm 2 end: [2.00482337243107;5.19552220912226;0.0684300716081022]
Robot Arm 1 togoal: 19.4198259211576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001448e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000181e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [3.56183798936194;-6.31912053122974;-2.29815123749928]
Robot Arm 1 end: [-5.82304867372952;-2.609656240453;0.370645418115913]
Robot Arm 1 togoal: 20.2656264521399
Mobile Platform 2 Pose: [-10.3415683311733;1.51270368664584;5.16499320683932]
Robot Arm 2 end: [2.19204251003109;5.18855192967508;0.151171237333372]
Robot Arm 1 togoal: 19.5247135990741

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954397e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000017e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [3.57152808978032;-6.30445244045761;-2.290693597693]
Robot Arm 1 end: [-5.74293938227651;-2.57717793902582;0.367050117136419]
Robot Arm 1 togoal: 20.1830905868282
Mobile Platform 2 Pose: [-10.4770909934328;1.5972352904951;5.16735205648082]
Robot Arm 2 end: [2.22204254474814;5.17153540842194;0.0903368993777745]
Robot Arm 1 togoal: 19.5346002652247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.925603e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000826e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [3.56183492965267;-6.27731117672606;-2.2845760576593]
Robot Arm 1 end: [-5.64214440361624;-2.53522480025544;0.36593268088413]
Robot Arm 1 togoal: 20.0784143509444
Mobile Platform 2 Pose: [-10.7130257460473;1.75430371947729;5.18396606478364]
Robot Arm 2 end: [2.43239636587735;5.19969840932075;0.155521274458169]
Robot Arm 1 togoal: 19.6839996769583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945807e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000305e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [3.46311422580241;-6.21394745236554;-2.28519262968792]
Robot Arm 1 end: [-5.53330428679048;-2.6122998324304;0.445463969340902]
Robot Arm 1 togoal: 20.0377250924947
Mobile Platform 2 Pose: [-10.8593732175273;1.8475366211706;5.18841668720057]
Robot Arm 2 end: [2.48261313359361;5.20772138783449;0.107391251898511]
Robot Arm 1 togoal: 19.7253316865819

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942313e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001683e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [3.35542321136075;-6.15026158189716;-2.28814806864256]
Robot Arm 1 end: [-5.50029949281566;-2.77577298841333;0.578588886737963]
Robot Arm 1 togoal: 20.1088984910873
Mobile Platform 2 Pose: [-11.0701260251591;1.98818383993845;5.20860468548071]
Robot Arm 2 end: [2.67219958423655;5.22096153299744;0.169108467711066]
Robot Arm 1 togoal: 19.8516982398682

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.925262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.016332e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [3.2488096636976;-6.08528541568404;-2.29147081238342]
Robot Arm 1 end: [-5.49042201723073;-2.87941738265146;0.727649069795454]
Robot Arm 1 togoal: 20.1609020653072
Mobile Platform 2 Pose: [-11.2306491735972;2.08907182750657;5.213982383964]
Robot Arm 2 end: [2.77323535551352;5.2252880877285;0.160700328126663]
Robot Arm 1 togoal: 19.9202247477022

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.894009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [3.19238813819972;-6.04568316431313;-2.29222600723527]
Robot Arm 1 end: [-5.39866688437463;-2.81432170607233;0.857631659643349]
Robot Arm 1 togoal: 20.0440973894471
Mobile Platform 2 Pose: [-11.3704822711139;2.17524435886639;5.21638823010214]
Robot Arm 2 end: [2.86291268712023;5.22540294815504;0.160616459651422]
Robot Arm 1 togoal: 19.9779393608805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.477979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [3.39844029487615;-6.17738607687713;-2.28862604849956]
Robot Arm 1 end: [-5.48448736898209;-2.69987635837103;0.96907938923102]
Robot Arm 1 togoal: 20.0339917380104
Mobile Platform 2 Pose: [-11.4960447529801;2.25195668564472;5.21729775626037]
Robot Arm 2 end: [2.94204542679368;5.22432617783569;0.16087180875244]
Robot Arm 1 togoal: 20.0281450987092

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.842246e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [3.59690519060839;-6.29370675247368;-2.28560988354281]
Robot Arm 1 end: [-5.57673085131832;-2.61099894041953;1.02664856864499]
Robot Arm 1 togoal: 20.0478483041786
Mobile Platform 2 Pose: [-11.6135911037001;2.32342430770544;5.21745443289834]
Robot Arm 2 end: [3.01535830188489;5.22267048880653;0.16128695206806]
Robot Arm 1 togoal: 20.074313639822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967809e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [3.68987231693261;-6.34117643961878;-2.28084969489122]
Robot Arm 1 end: [-5.67083029844126;-2.57021809758416;1.13280892846268]
Robot Arm 1 togoal: 20.0931828631354
Mobile Platform 2 Pose: [-11.7257765044801;2.39141673945921;5.217146060647]
Robot Arm 2 end: [3.08481677164777;5.22067181678487;0.161768740347546]
Robot Arm 1 togoal: 20.1178724053783

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967426e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [3.7997587301444;-6.3947417381269;-2.27424001283118]
Robot Arm 1 end: [-5.79631945305051;-2.49667861867353;1.23947830360279]
Robot Arm 1 togoal: 20.1437320084133
Mobile Platform 2 Pose: [-11.8340373297663;2.45688377027782;5.21652026774931]
Robot Arm 2 end: [3.15147350013076;5.21844948901559;0.162267481815339]
Robot Arm 1 togoal: 20.1595787190688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [3.92923151417891;-6.45212767337807;-2.26521686263262]
Robot Arm 1 end: [-5.98094104757943;-2.4622215708772;1.3300190196697]
Robot Arm 1 togoal: 20.2665714165641
Mobile Platform 2 Pose: [-11.9392546651962;2.52040441267244;5.21566458129771]
Robot Arm 2 end: [3.21597077137752;5.21607298763892;0.162755611495091]
Robot Arm 1 togoal: 20.1998908020307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.890025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [4.06337795826946;-6.48933698499009;-2.26042611963242]
Robot Arm 1 end: [-6.081653778823;-2.38260681292855;1.38466115577064]
Robot Arm 1 togoal: 20.2969733710709
Mobile Platform 2 Pose: [-12.0420131923331;2.58236090458633;5.21463573349884]
Robot Arm 2 end: [3.27873390561771;5.21358636321744;0.163217972224491]
Robot Arm 1 togoal: 20.2391095703894

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.766847e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [4.16269209375564;-6.47154795536554;-2.27836639215215]
Robot Arm 1 end: [-6.01520498298624;-2.42652859381752;1.34059144572434]
Robot Arm 1 togoal: 20.2716045439548
Mobile Platform 2 Pose: [-12.1427224043391;2.6430197548251;5.21347275761616]
Robot Arm 2 end: [3.34008789132155;5.21114079164307;0.163685760010203]
Robot Arm 1 togoal: 20.2775504210309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.892761e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [4.16297426814529;-6.43887216447789;-2.30371552003425]
Robot Arm 1 end: [-5.93667255042336;-2.58307635042596;1.25121818831673]
Robot Arm 1 togoal: 20.3072431572083
Mobile Platform 2 Pose: [-12.2416652124611;2.70255804458469;5.21220496494275]
Robot Arm 2 end: [3.40024058423169;5.20881770220508;0.164179998726673]
Robot Arm 1 togoal: 20.3154016482582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [4.17805204906603;-6.4501468580015;-2.29445608273961]
Robot Arm 1 end: [-5.93621057730287;-2.70205945891923;1.14127728939081]
Robot Arm 1 togoal: 20.3825737187189
Mobile Platform 2 Pose: [-12.3390602292706;2.76110766914263;5.21085652025541]
Robot Arm 2 end: [3.45932280786401;5.20659301885661;0.164691723546501]
Robot Arm 1 togoal: 20.3527249730399

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [4.19325357893369;-6.47315883530832;-2.23452331181168]
Robot Arm 1 end: [-5.90917317086538;-2.64654726027769;1.03940082265527]
Robot Arm 1 togoal: 20.3290158727529
Mobile Platform 2 Pose: [-12.4351073264993;2.81879052842554;5.20944868965587]
Robot Arm 2 end: [3.51745447454144;5.20444418056061;0.165209501271662]
Robot Arm 1 togoal: 20.3895773910968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [4.25491018418995;-6.51155717148095;-2.18916564324762]
Robot Arm 1 end: [-5.87640264413889;-2.52931771795478;0.944260783393574]
Robot Arm 1 togoal: 20.2330569000398
Mobile Platform 2 Pose: [-12.5299938742628;2.87572255499467;5.20800067782229]
Robot Arm 2 end: [3.57474971387189;5.20235077450528;0.165720054302047]
Robot Arm 1 togoal: 20.426014848633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [4.29300648572632;-6.53080613428114;-2.18222343630721]
Robot Arm 1 end: [-5.85920373180408;-2.48892051821904;0.817296813806321]
Robot Arm 1 togoal: 20.1985810613399
Mobile Platform 2 Pose: [-12.6238964631209;2.93201469783203;5.20652992097959]
Robot Arm 2 end: [3.63131868977739;5.20029486101032;0.166209163801591]
Robot Arm 1 togoal: 20.4620935217729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [4.31540331584997;-6.53952504851059;-2.1829037658893]
Robot Arm 1 end: [-5.83494702192594;-2.41894670165992;0.664060412623446]
Robot Arm 1 togoal: 20.1422473875753
Mobile Platform 2 Pose: [-12.7169797032036;2.98777193635107;5.20505205350668]
Robot Arm 2 end: [3.6872672652912;5.19826101015738;0.16666279551117]
Robot Arm 1 togoal: 20.4978694548444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.351022e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [4.29040189676112;-6.51928674930108;-2.19377795877447]
Robot Arm 1 end: [-5.81355283965466;-2.34357011085646;0.476301060754758]
Robot Arm 1 togoal: 20.0878961657969
Mobile Platform 2 Pose: [-12.8093932871796;3.0430911250668;5.20358069760161]
Robot Arm 2 end: [3.74269530581367;5.19623611558685;0.167068310991517]
Robot Arm 1 togoal: 20.5333971219475

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.102410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [4.1596837360471;-6.42492791106366;-2.22047841085971]
Robot Arm 1 end: [-5.77985348333937;-2.28935756849906;0.297914405975366]
Robot Arm 1 togoal: 20.0381641816907
Mobile Platform 2 Pose: [-12.9012683167796;3.09805833734831;5.2021271861172]
Robot Arm 2 end: [3.79769425598361;5.19420907571079;0.167415597316796]
Robot Arm 1 togoal: 20.5687274023748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.446681e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [3.94672825506113;-6.26502560278795;-2.25122526872089]
Robot Arm 1 end: [-5.67658827760586;-2.17447581429253;0.209315346065998]
Robot Arm 1 togoal: 19.8920536398264
Mobile Platform 2 Pose: [-12.9927136954672;3.15274624937212;5.20070030100053]
Robot Arm 2 end: [3.85234448843285;5.19217043465108;0.167697927412113]
Robot Arm 1 togoal: 20.6039053783833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.225237e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [3.69336977524968;-6.09475538788428;-2.26932576905138]
Robot Arm 1 end: [-5.47859407029155;-2.13594734361023;0.241941695609898]
Robot Arm 1 togoal: 19.7105069053481
Mobile Platform 2 Pose: [-13.0838133001283;3.207212033588;5.19930608889036]
Robot Arm 2 end: [3.90671289497565;5.19011206888384;0.16791243832713]
Robot Arm 1 togoal: 20.6389683404497
Step 381:
Mobile Platform 1 Pose: [3.60389681888114;-6.04894650968165;-2.27779783920035]
Robot Arm 1 end: [-5.41647886696402;-2.24166868881492;0.224530720895755]
Robot Arm 1 togoal: 19.7283325583771
Mobile Platform 2 Pose: [-13.1746244240505;3.26149609330027;5.19794779735794]
Robot Arm 2 end: [3.96085108126548;5.18802697254045;0.168060157099122]
Robot Arm 1 togoal: 20.6739442886547
Step 382:
Mobile Platform 1 Pose: [3.56794663284265;-6.03111199319836;-2.28134936032542]
Robot Arm 1 end: [-5.39046548262308;-2.33030685922159;0.213932557644175]
Robot Arm 1 togoal: 19.7638893012799
Mobile Platform 2 Pose: [-13.2651777942255;3.31562184184403;5.19662595807501]
Robot Arm 2 end: [4.01479444819638;5.18590916109517;0.168145601637493]
Robot Arm 1 togoal: 20.7088511390033
Step 383:
Mobile Platform 1 Pose: [3.55625130393111;-6.02535808443944;-2.2824995136258]
Robot Arm 1 end: [-5.38355996084855;-2.41066563675681;0.20744323930627]
Robot Arm 1 togoal: 19.8091890288311
Mobile Platform 2 Pose: [-13.3554792272913;3.36959657572559;5.1953386212514]
Robot Arm 2 end: [4.0685623024484;5.1837536815669;0.168176045337581]
Robot Arm 1 togoal: 20.7436967238888
Step 384:
Mobile Platform 1 Pose: [3.55570296715652;-6.02526239687212;-2.28255396261721]
Robot Arm 1 end: [-5.3898232626041;-2.48740294813326;0.204186852707165]
Robot Arm 1 togoal: 19.8624240541813
Mobile Platform 2 Pose: [-13.4455127433636;3.4234133309744;5.19408172832635]
Robot Arm 2 end: [4.1221590077659;5.18155668147746;0.16816060120698]
Robot Arm 1 togoal: 20.7784795582464
Step 385:
Mobile Platform 1 Pose: [3.55952073983795;-6.02757050144575;-2.2821197262283]
Robot Arm 1 end: [-5.40645396906092;-2.56159905326261;0.203597944975087]
Robot Arm 1 togoal: 19.9220516727145
Mobile Platform 2 Pose: [-13.535244776292;3.47705348957535;5.1928495863595]
Robot Arm 2 end: [4.17557601313222;5.17931549879022;0.168109262933932]
Robot Arm 1 togoal: 20.8131902326948
Step 386:
Mobile Platform 1 Pose: [3.56412787879312;-6.03052643176201;-2.28147490458026]
Robot Arm 1 end: [-5.43155312104321;-2.63268839279528;0.205318964408712]
Robot Arm 1 togoal: 19.9862118557884
Mobile Platform 2 Pose: [-13.6246289853485;3.53048981614048;5.19163539759446]
Robot Arm 2 end: [4.22879450731071;5.17702871487454;0.168032049270439]
Robot Arm 1 togoal: 20.8478132191149
Step 387:
Mobile Platform 1 Pose: [3.63349948070314;-6.0661029748575;-2.28076376810955]
Robot Arm 1 end: [-5.49589920364915;-2.6990068658178;0.209150230350304]
Robot Arm 1 togoal: 20.0775650248199
Mobile Platform 2 Pose: [-13.7136111518546;3.5836895849088;5.19043179096602]
Robot Arm 2 end: [4.28178837042463;5.17469614923278;0.167938336919599]
Robot Arm 1 togoal: 20.8823288486518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.143196e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [3.6655941318254;-6.08325489251707;-2.28007935576219]
Robot Arm 1 end: [-5.4372892743349;-2.5966401422106;0.215025834002966]
Robot Arm 1 togoal: 19.9672434635855
Mobile Platform 2 Pose: [-13.8021337173011;3.63661750245945;5.18923130659152]
Robot Arm 2 end: [4.33452709995527;5.17231878162392;0.167836411919443]
Robot Arm 1 togoal: 20.9167152317633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.918570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [3.68700878596447;-6.09516576010375;-2.2803175940419]
Robot Arm 1 end: [-5.39203030106608;-2.48011150250086;0.222453977047421]
Robot Arm 1 togoal: 19.8583484126328
Mobile Platform 2 Pose: [-13.8901396260614;3.68923820262017;5.18802679856425]
Robot Arm 2 end: [4.38697845208857;5.16989860786773;0.167733240495201]
Robot Arm 1 togoal: 20.9509499468694

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [3.68704624810657;-6.09614288192241;-2.28116628029128]
Robot Arm 1 end: [-5.35484613038728;-2.26703232908434;0.201676455529916]
Robot Arm 1 togoal: 19.6975124435971
Mobile Platform 2 Pose: [-13.9775753189011;3.74151820322185;5.18681173730262]
Robot Arm 2 end: [4.43911062720483;5.16743844865755;0.167634410472766]
Robot Arm 1 togoal: 20.985011394264

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992656e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [3.68164547552371;-6.09505572068885;-2.2822274317745]
Robot Arm 1 end: [-5.38503167626208;-2.05477530749679;0.187320428337714]
Robot Arm 1 togoal: 19.5906995432448
Mobile Platform 2 Pose: [-14.0643928199374;3.79342728672748;5.18558040773103]
Robot Arm 2 end: [4.49089391835193;5.16494173406958;0.167544198973948]
Robot Arm 1 togoal: 21.0188797740504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [3.64708760111733;-6.07317199504025;-2.28413165493412]
Robot Arm 1 end: [-5.38413225513479;-1.97806677860244;0.200506816569458]
Robot Arm 1 togoal: 19.5419884158093
Mobile Platform 2 Pose: [-14.1505510180534;3.84493936088259;5.18432801339439]
Robot Arm 2 end: [4.54230183036466;5.16241228562134;0.167465705154303]
Robot Arm 1 togoal: 21.0525377076473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [3.64311111235161;-6.05150397966157;-2.2806103763986]
Robot Arm 1 end: [-5.47442846275393;-1.81424923725907;0.193419993123725]
Robot Arm 1 togoal: 19.514017781165
Mobile Platform 2 Pose: [-14.23601625646;3.89603287514759;5.18305070383499]
Robot Arm 2 end: [4.59331174018494;5.15985410976925;0.167401026963474]
Robot Arm 1 togoal: 21.0859705575891

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.578519e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [3.66220342174174;-6.03224387315601;-2.27211580936409]
Robot Arm 1 end: [-5.53637398741329;-1.75940224021122;0.198067590879521]
Robot Arm 1 togoal: 19.5297877070632
Mobile Platform 2 Pose: [-14.3207624067175;3.94669090167943;5.18174554418957]
Robot Arm 2 end: [4.643905173033;5.1572712203493;0.167351427422459]
Robot Arm 1 togoal: 21.1191665086966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.595437e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [3.72218929100328;-6.02918102678427;-2.25353724026226]
Robot Arm 1 end: [-5.62641589656612;-1.73125061724694;0.182419789960921]
Robot Arm 1 togoal: 19.5856812951661
Mobile Platform 2 Pose: [-14.4047705629859;3.99690097193491;5.18041044672094]
Robot Arm 2 end: [4.69406780402639;5.15466748942488;0.167317496147118]
Robot Arm 1 togoal: 21.1521164819408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.696444e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [3.79002355168362;-6.03240569299332;-2.22774363528905]
Robot Arm 1 end: [-5.77412721396703;-1.67081340446395;0.174964191399248]
Robot Arm 1 togoal: 19.6683193812351
Mobile Platform 2 Pose: [-14.4880285012439;4.04665475898349;5.17904407943195]
Robot Arm 2 end: [4.74378926404457;5.15204653227714;0.167299288406946]
Robot Arm 1 togoal: 21.1848139381826

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997307e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [3.86826309321562;-6.05292672329271;-2.2038266232969]
Robot Arm 1 end: [-5.77275756880773;-1.61788531985019;0.220955171293977]
Robot Arm 1 togoal: 19.6327597954597
Mobile Platform 2 Pose: [-14.5705300047914;4.09594767272792;5.17764576510509]
Robot Arm 2 end: [4.79306283149592;5.14941162305269;0.167296440640686]
Robot Arm 1 togoal: 21.2172546241943

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996473e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [3.96611751302505;-6.07417557035193;-2.16877836574161]
Robot Arm 1 end: [-5.81834633072283;-1.51061348116232;0.327101888286841]
Robot Arm 1 togoal: 19.5994820353131
Mobile Platform 2 Pose: [-14.6522741235146;4.14477841156077;5.17621537873231]
Robot Arm 2 end: [4.8418850543606;5.14676564119172;0.167308263219485]
Robot Arm 1 togoal: 21.2494362907411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.947838e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [3.91101028786047;-5.99378234409063;-2.10334489510117]
Robot Arm 1 end: [-5.79191659528314;-1.47928775415404;0.224985314478348]
Robot Arm 1 togoal: 19.5662549089022
Mobile Platform 2 Pose: [-14.7332644345341;4.19314851207152;5.1747532493427]
Robot Arm 2 end: [4.89025535637286;5.14411103929452;0.167333819351473]
Robot Arm 1 togoal: 21.2813584124018

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967852e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.35258794378618;9.99566059907306;9.81851214533108]
Robot Arm 2 end: [9.77362002791198;9.83021151737349;1.48915678286957]
Robot Arm 1 togoal: 28.0041845412876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.15544762582582;9.86196972884621;9.81488851039254]
Robot Arm 2 end: [9.54849630174836;9.88355921327199;1.46005029349801]
Robot Arm 1 togoal: 27.8837470493814

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883084e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-2.00113060899477;9.74656908502156;9.8280782370857]
Robot Arm 2 end: [9.24996333438607;9.99349454928848;1.34368241244873]
Robot Arm 1 togoal: 27.7548618118428

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.89655830144018;9.65435742214306;9.85226728069294]
Robot Arm 2 end: [9.00445772901659;9.99249287961833;1.20711988334486]
Robot Arm 1 togoal: 27.5856431109227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928035e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.75788068234521;9.52864190465855;9.87459777603679]
Robot Arm 2 end: [8.81476836810365;10.035680907724;1.12812811739535]
Robot Arm 1 togoal: 27.4877857692771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821187e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.59158350835172;9.44687207425229;9.86664746485362]
Robot Arm 2 end: [8.67992349289611;10.000642788055;1.04218902584006]
Robot Arm 1 togoal: 27.371417821969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.879956e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.50637067492227;9.39917180953767;9.86197675585747]
Robot Arm 2 end: [8.63459597516248;9.83621413781051;1.06174100878587]
Robot Arm 1 togoal: 27.2201116327547

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919215e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.44299665897119;9.36139303460196;9.85469430872453]
Robot Arm 2 end: [8.65120409216765;9.70044470663403;1.10864889854522]
Robot Arm 1 togoal: 27.1319916179721

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.953695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.38106937401154;9.32567011788643;9.84423408464485]
Robot Arm 2 end: [8.66577565362521;9.60323865418266;1.11593415330352]
Robot Arm 1 togoal: 27.0714282006781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.31990375760479;9.29071418881051;9.82965898711008]
Robot Arm 2 end: [8.63669451184883;9.51400756142535;1.05118959039068]
Robot Arm 1 togoal: 26.9878416190758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.26683387310231;9.26148484959282;9.8157565350536]
Robot Arm 2 end: [8.62820157260443;9.3777839295204;0.976000368833963]
Robot Arm 1 togoal: 26.8850409790785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.19250930447057;9.22448954522453;9.80289973899437]
Robot Arm 2 end: [8.67035252848452;9.22498815318255;0.899385022848688]
Robot Arm 1 togoal: 26.8061306086447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.06448514655318;9.16120546127958;9.78179579476625]
Robot Arm 2 end: [8.73645114114976;9.08128184183994;0.81665212229908]
Robot Arm 1 togoal: 26.7515611511785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.849870870646054;9.04893399041111;9.73053959745598]
Robot Arm 2 end: [8.80005394084464;8.94905820643621;0.730036740316628]
Robot Arm 1 togoal: 26.7045774721815

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993615e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.660439678757058;8.90713557031844;9.57486222110091]
Robot Arm 2 end: [8.75380263839732;8.76896768864593;0.650840949083009]
Robot Arm 1 togoal: 26.546872476269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.885185774433004;8.95939356293701;9.43042829406428]
Robot Arm 2 end: [8.69047297521705;8.63459555831471;0.72907530652204]
Robot Arm 1 togoal: 26.4047169342794

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-0.839689665999013;8.90808197316314;9.27378670410441]
Robot Arm 2 end: [8.74368940625475;8.44370783002612;0.778659899120091]
Robot Arm 1 togoal: 26.3067922769555

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.01465898496507;8.98553905275885;9.23011591695338]
Robot Arm 2 end: [8.69878039286632;8.4526853101913;0.896704056163489]
Robot Arm 1 togoal: 26.2780465172562

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.18699158610503;9.0598971475507;9.13208096760952]
Robot Arm 2 end: [8.71342453939168;8.4628666504295;1.06946429326663]
Robot Arm 1 togoal: 26.2920822195106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.29396847180089;9.07905582228415;8.94285853452641]
Robot Arm 2 end: [8.76439287406318;8.31269016997983;1.14890513158508]
Robot Arm 1 togoal: 26.222042555548

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.384098e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.48358459489595;9.15764814261775;8.82453259165308]
Robot Arm 2 end: [8.79274052415402;8.26277049539307;1.1805355429874]
Robot Arm 1 togoal: 26.2070944330361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.682441e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.70482005999038;9.25070733737107;8.7247085098352]
Robot Arm 2 end: [9.00415847420041;8.1666150804783;1.24306740139324]
Robot Arm 1 togoal: 26.2918464418944
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.94637365826978;9.3629071158047;8.64796872568231]
Robot Arm 2 end: [9.26034662896915;8.21616151061327;1.35688671856472]
Robot Arm 1 togoal: 26.5106851216247
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.7087220619212;9.27154200844186;8.54910479721871]
Robot Arm 2 end: [9.05216731632813;8.07962093892689;1.32267312197459]
Robot Arm 1 togoal: 26.2658924519079
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.47270265742278;9.18407846422377;8.4471974462622]
Robot Arm 2 end: [8.98818771339008;7.89706905270172;1.28251766112261]
Robot Arm 1 togoal: 26.0943140768649
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.356795362348;9.15002315196759;8.34767644211869]
Robot Arm 2 end: [9.02536996844973;7.80354143155307;1.31520434819111]
Robot Arm 1 togoal: 26.0571130747771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.003833e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-0.656497331022536;8.90342397548701;8.30369089330341]
Robot Arm 2 end: [8.83268126565543;7.67755496004228;1.23721687283524]
Robot Arm 1 togoal: 25.8311123624661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.986898e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-0.671178133866576;8.92389220096933;8.17693372442855]
Robot Arm 2 end: [8.92536189318666;7.57217918317049;1.27208288517739]
Robot Arm 1 togoal: 25.8265902465576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.744352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-0.691135388306614;8.96022598170612;8.0949722154651]
Robot Arm 2 end: [8.90988391369016;7.36229218548533;1.10367468073899]
Robot Arm 1 togoal: 25.6750279451746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.874739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-0.705075110319484;8.98646404822392;8.03897804307261]
Robot Arm 2 end: [8.94877664408286;7.25532909536709;0.994819582368913]
Robot Arm 1 togoal: 25.6335597279945

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955764e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-0.802622080355453;9.04826073357288;7.99320547567384]
Robot Arm 2 end: [9.07694623436677;7.23162679015377;1.01048893642904]
Robot Arm 1 togoal: 25.7122434067915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-0.898818027963096;9.11379504419143;7.94679532902716]
Robot Arm 2 end: [9.26032908209244;7.27472213338848;1.11279877629252]
Robot Arm 1 togoal: 25.8755276153997

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986384e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-0.912727372159382;9.15861420735154;7.87996098202811]
Robot Arm 2 end: [9.3933664993162;7.20214247625617;1.06755036370461]
Robot Arm 1 togoal: 25.9272440981079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.929984e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-0.970179828181309;9.22070612581997;7.82426611479381]
Robot Arm 2 end: [9.32504578247556;7.25831116284717;1.22786267803949]
Robot Arm 1 togoal: 25.9112452070131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917275e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-0.840761483178845;9.18091244065578;7.78134734291442]
Robot Arm 2 end: [9.13975583609079;7.07449575803083;1.08333625595945]
Robot Arm 1 togoal: 25.6526842339945

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.882800e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-0.702507630901604;9.13213259848276;7.6730302888178]
Robot Arm 2 end: [8.96340175161329;6.93578144382398;0.986765384897703]
Robot Arm 1 togoal: 25.4306253507466

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.864849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-0.50400184595477;9.01001630056858;7.53061893058282]
Robot Arm 2 end: [8.91450033496337;6.74983609291162;0.836310644087779]
Robot Arm 1 togoal: 25.2741791473993

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.026478e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-0.643420372312613;8.99703119890304;7.37179802526466]
Robot Arm 2 end: [8.80953420212426;6.58732379334034;0.768443534869768]
Robot Arm 1 togoal: 25.089922852795

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.843550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-0.40014159446477;8.85723359582531;7.26584488199154]
Robot Arm 2 end: [8.77421064658506;6.48527926107159;0.597725265326823]
Robot Arm 1 togoal: 25.0017570173288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.834621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-0.322001610183551;8.73381498899708;7.09208993406442]
Robot Arm 2 end: [8.5701391841427;6.35196121328283;0.506650720638376]
Robot Arm 1 togoal: 24.7642001546523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.559050e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [0.00741391167364525;8.53239918622286;7.00959874296357]
Robot Arm 2 end: [8.55053443896194;6.26848822307591;0.551737564251498]
Robot Arm 1 togoal: 24.6926204908435

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.243809e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [0.354592603597721;8.31257308775306;6.85210999964701]
Robot Arm 2 end: [8.37217467916161;6.15102963733493;0.398074829484893]
Robot Arm 1 togoal: 24.4878239483682

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989185e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [0.577128334706698;8.22596931890809;6.69918337745572]
Robot Arm 2 end: [8.436671205861;6.07783497893918;0.314259791119844]
Robot Arm 1 togoal: 24.4921180116125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.460614e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [0.557274050892945;8.27143230307209;6.49260538235248]
Robot Arm 2 end: [8.55917423326677;5.92269132491834;0.292245735308857]
Robot Arm 1 togoal: 24.484380414713

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.333519e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [0.524722630594683;8.31858538103253;6.41327378824276]
Robot Arm 2 end: [8.55327568585403;5.84788757342391;0.20805886976192]
Robot Arm 1 togoal: 24.4356991638598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.923188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [0.628268764863017;8.30124284829064;6.25301241129947]
Robot Arm 2 end: [8.51641807980455;5.6972235117797;0.18223539831969]
Robot Arm 1 togoal: 24.3115786230264

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.500739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.224100e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307231657056;-6.24962959890427;-7.10439671995214]
Robot Arm 1 end: [-6.13860100147105;-6.93244562099744;1.24046691431429]
Robot Arm 1 togoal: 23.3931796856452
Mobile Platform 2 Pose: [0.6875990134319;8.2984034609804;6.13134572620513]
Robot Arm 2 end: [8.44820698782843;5.5407250705167;0.206234186828541]
Robot Arm 1 togoal: 24.1573816032829

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.694632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954980e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407579899566;-6.2179345204005;-7.10337756508191]
Robot Arm 1 end: [-6.10318570686967;-6.8809382246652;1.17440061263298]
Robot Arm 1 togoal: 23.3323551402386
Mobile Platform 2 Pose: [0.870742522484165;8.22049580223362;6.02802988981883]
Robot Arm 2 end: [8.24552300437546;5.41123850840439;0.19208719878636]
Robot Arm 1 togoal: 23.9201043329529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.979994e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859323378779;-6.18978109641873;-7.10219141867284]
Robot Arm 1 end: [-6.07852945424791;-6.82863751273114;1.10279655707996]
Robot Arm 1 togoal: 23.2786821806066
Mobile Platform 2 Pose: [1.11952486421739;8.09026795642003;5.87137321809287]
Robot Arm 2 end: [8.01366820884187;5.30200959037874;0.122870589461144]
Robot Arm 1 togoal: 23.6769614961654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000068e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951050e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70845777347596;-6.16355485948904;-7.1010799128953]
Robot Arm 1 end: [-6.05861222080202;-6.77636395392592;1.02305412365795]
Robot Arm 1 togoal: 23.2287185197213
Mobile Platform 2 Pose: [1.32454899766921;7.9430824201845;5.68068866642499]
Robot Arm 2 end: [7.75623847033246;5.10647313497477;0.131537201933407]
Robot Arm 1 togoal: 23.3542317805626

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000207e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001919e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74537132127519;-6.13815764174955;-7.1000057096861]
Robot Arm 1 end: [-6.04161926148615;-6.72122950892557;0.93099116416016]
Robot Arm 1 togoal: 23.1793264623245
Mobile Platform 2 Pose: [1.46793664612159;7.79494542875082;5.48980809204638]
Robot Arm 2 end: [7.54439347199078;5.0044206230908;0.0728437158799068]
Robot Arm 1 togoal: 23.130835202342

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78014100100089;-6.11348879567584;-7.09883244027074]
Robot Arm 1 end: [-6.02899045809154;-6.65915264573105;0.81918479882294]
Robot Arm 1 togoal: 23.1289525607184
Mobile Platform 2 Pose: [1.60586209356733;7.61350341823476;5.29361378878205]
Robot Arm 2 end: [7.32810238661646;4.74741615959118;0.123417280816615]
Robot Arm 1 togoal: 22.7969878089671

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002510e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80657489818593;-6.09325547934326;-7.09790877102905]
Robot Arm 1 end: [-5.99677750942257;-6.60133433494384;0.729452961503064]
Robot Arm 1 togoal: 23.0678941193319
Mobile Platform 2 Pose: [1.55859988170164;7.52628138783465;5.10455825725234]
Robot Arm 2 end: [7.38302095504696;4.56922817255575;0.0713578978790281]
Robot Arm 1 togoal: 22.727368727854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964147e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82829077588873;-6.07557364645557;-7.09843364359655]
Robot Arm 1 end: [-5.9783341130212;-6.56332169827225;0.646553798733391]
Robot Arm 1 togoal: 23.030749941474
Mobile Platform 2 Pose: [1.67262962604791;7.38783675414653;4.8963922562462]
Robot Arm 2 end: [7.30854370646771;4.27645643697644;0.122690836548171]
Robot Arm 1 togoal: 22.4801726450706

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986885e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989432e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85392347192179;-6.05628511708956;-7.10074306591966]
Robot Arm 1 end: [-5.94931582423122;-6.53401994026288;0.572508245449144]
Robot Arm 1 togoal: 22.9924797978499
Mobile Platform 2 Pose: [1.89289484112399;7.23603518592401;4.70981813497969]
Robot Arm 2 end: [7.06971311642038;4.16698049621438;0.0597552522049014]
Robot Arm 1 togoal: 22.2309171756356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985845e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006965e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89411119234124;-6.02975262988996;-7.10426453057029]
Robot Arm 1 end: [-5.91486900940816;-6.51006849659471;0.51159457891626]
Robot Arm 1 togoal: 22.9539608053378
Mobile Platform 2 Pose: [1.97136019043045;7.12329633350209;4.50489915639368]
Robot Arm 2 end: [7.04469035453029;3.86813129305019;0.11604501586122]
Robot Arm 1 togoal: 22.0186383173563

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002025e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94420139788106;-5.99893697722476;-7.10840854482976]
Robot Arm 1 end: [-5.88609187453562;-6.48975439379295;0.462701375563436]
Robot Arm 1 togoal: 22.9216035868411
Mobile Platform 2 Pose: [2.04837782219696;7.05291876687451;4.318468801186]
Robot Arm 2 end: [6.94943636911364;3.72192936106001;0.0846323660495901]
Robot Arm 1 togoal: 21.8549282909007

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99614663600416;-5.96853134318378;-7.11344244903349]
Robot Arm 1 end: [-5.86047559124835;-6.4730645176934;0.424861469224793]
Robot Arm 1 togoal: 22.893624882559
Mobile Platform 2 Pose: [2.07990987976042;6.99848778254231;4.13550642589418]
Robot Arm 2 end: [6.99936064620168;3.46033741812151;0.14303741386317]
Robot Arm 1 togoal: 21.7268894846479

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000941e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.04613604307375;-5.94113098415457;-7.11960768644343]
Robot Arm 1 end: [-5.81129918801722;-6.46904643195365;0.388965787530791]
Robot Arm 1 togoal: 22.8584376948435
Mobile Platform 2 Pose: [2.21918099120535;6.92677821593466;3.98160769130887]
Robot Arm 2 end: [6.80936002751595;3.30460558579109;0.169902183250784]
Robot Arm 1 togoal: 21.4800507364051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.745870e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969538e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.07845579533067;-5.92504081452838;-7.12417695264111]
Robot Arm 1 end: [-5.76276830380969;-6.47540782364784;0.373997511854522]
Robot Arm 1 togoal: 22.8302330750896
Mobile Platform 2 Pose: [2.20015473138752;6.91035002985682;3.81111364274658]
Robot Arm 2 end: [6.91872503698546;3.16563335584694;0.117181701250424]
Robot Arm 1 togoal: 21.4836650586815

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.825781e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001909e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.11914953222954;-5.89818426606588;-7.12348824629046]
Robot Arm 1 end: [-5.76228130503516;-6.48296230090766;0.38021701530403]
Robot Arm 1 togoal: 22.8350376389087
Mobile Platform 2 Pose: [2.37665057608508;6.82951383622084;3.70096590442631]
Robot Arm 2 end: [6.71979602056128;3.03972991618657;0.174436502972659]
Robot Arm 1 togoal: 21.2461612652214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.404562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955105e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.16486420694109;-5.87026780497262;-7.12400403465424]
Robot Arm 1 end: [-5.64420920386364;-6.55289510896287;0.346716795364548]
Robot Arm 1 togoal: 22.8061079350964
Mobile Platform 2 Pose: [2.4031770728862;6.80119972812411;3.54485359183993]
Robot Arm 2 end: [6.81221085786386;2.91480346322077;0.122844683787015]
Robot Arm 1 togoal: 21.246115426382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.908583e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001756e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.1971864767316;-5.84992005880737;-7.12839836414616]
Robot Arm 1 end: [-5.65672342471273;-6.55964548306161;0.348792693551343]
Robot Arm 1 togoal: 22.8194854651003
Mobile Platform 2 Pose: [2.56307492843973;6.73410656365239;3.44846670386045]
Robot Arm 2 end: [6.6278899998005;2.79981267999423;0.181253002496554]
Robot Arm 1 togoal: 21.026574911424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.474907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954634e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.2207758480132;-5.83469559440149;-7.13217793327436]
Robot Arm 1 end: [-5.68130287319723;-6.56859044157152;0.334318156449118]
Robot Arm 1 togoal: 22.843591285507
Mobile Platform 2 Pose: [2.6657040781797;6.67735732231591;3.31515725246188]
Robot Arm 2 end: [6.59452245083216;2.70537704799712;0.131465767131809]
Robot Arm 1 togoal: 20.9460160099065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.946194e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001976e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.24014342151601;-5.82217665784631;-7.13535531339402]
Robot Arm 1 end: [-5.72707538029732;-6.57769297275374;0.315785064640384]
Robot Arm 1 togoal: 22.8825992264178
Mobile Platform 2 Pose: [2.8382999706286;6.59743432619431;3.22288392844121]
Robot Arm 2 end: [6.42299669818522;2.60281492180899;0.191432944899207]
Robot Arm 1 togoal: 20.7439994469722

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.941438e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.25825874179791;-5.81070538609208;-7.13790676959861]
Robot Arm 1 end: [-5.77231838208499;-6.59225797322827;0.294448158112724]
Robot Arm 1 togoal: 22.9253894452603
Mobile Platform 2 Pose: [3.01255216163199;6.50314536064076;3.10714466884104]
Robot Arm 2 end: [6.28478634804038;2.54722220212622;0.145595276657277]
Robot Arm 1 togoal: 20.6038537546704

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982139e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.27390894022145;-5.80074016015758;-7.14010798162253]
Robot Arm 1 end: [-5.84806580522241;-6.61185218288927;0.271764076960542]
Robot Arm 1 togoal: 22.9929426804016
Mobile Platform 2 Pose: [3.23782599676299;6.39554210751837;3.04465786986434]
Robot Arm 2 end: [6.13746124438109;2.46709799225631;0.208512786875244]
Robot Arm 1 togoal: 20.4345004941897

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937454e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.28688767425922;-5.79249845396411;-7.14170356176601]
Robot Arm 1 end: [-5.93340147838367;-6.64457180217752;0.248859095847789]
Robot Arm 1 togoal: 23.076685109201
Mobile Platform 2 Pose: [3.45812926578506;6.29320182003332;2.98564480376294]
Robot Arm 2 end: [6.12947873543678;2.36731263872914;0.226588612276811]
Robot Arm 1 togoal: 20.3662958591307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937914e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.953886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.29826911205764;-5.78529508745651;-7.14293496418781]
Robot Arm 1 end: [-6.04298504706871;-6.66892665940798;0.261326139777243]
Robot Arm 1 togoal: 23.1693172845322
Mobile Platform 2 Pose: [3.66544915151978;6.19870448244609;2.91896812397386]
Robot Arm 2 end: [6.08292406879842;2.27372924922582;0.247760673864556]
Robot Arm 1 togoal: 20.2713101369313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.915792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.30525563642801;-5.7809794195713;-7.14379380014694]
Robot Arm 1 end: [-6.18921544539623;-6.70107391677313;0.327213898752394]
Robot Arm 1 togoal: 23.2903776690398
Mobile Platform 2 Pose: [3.86444023421363;6.11176250382287;2.85762352458403]
Robot Arm 2 end: [6.11419633169501;2.16113463349092;0.332465731306466]
Robot Arm 1 togoal: 20.2230841539604

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000055e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951862e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.29971275118229;-5.78418563226682;-7.14452227554745]
Robot Arm 1 end: [-6.24330571643592;-6.72169213567579;0.366295589351873]
Robot Arm 1 togoal: 23.3408078427867
Mobile Platform 2 Pose: [3.98447593265793;6.06807860992364;2.76433928828822]
Robot Arm 2 end: [6.16104297771676;2.10481094247923;0.265012614531914]
Robot Arm 1 togoal: 20.230750415969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000073e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998344e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.28813668255496;-5.790752784813;-7.14600130670931]
Robot Arm 1 end: [-6.30902613810821;-6.76548676123224;0.388797189777868]
Robot Arm 1 togoal: 23.4168264954503
Mobile Platform 2 Pose: [4.03795640202743;6.0574364468917;2.64779591607313]
Robot Arm 2 end: [6.16882025182873;2.00049352653071;0.235693829563805]
Robot Arm 1 togoal: 20.1765855676329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000903e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.27447818219797;-5.79817432845821;-7.1486640752059]
Robot Arm 1 end: [-6.38811900014688;-6.78950714519603;0.468719785484847]
Robot Arm 1 togoal: 23.4854271451772
Mobile Platform 2 Pose: [4.15272306236624;6.0185255184114;2.53882091258958]
Robot Arm 2 end: [6.20548471558629;1.89799204455778;0.25041699295424]
Robot Arm 1 togoal: 20.1443374360453

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965322e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.26146055113929;-5.80470946412966;-7.15196500996857]
Robot Arm 1 end: [-6.4648601005659;-6.83600264890565;0.610483168844786]
Robot Arm 1 togoal: 23.5663243681526
Mobile Platform 2 Pose: [4.10297026539981;6.05674088660585;2.42229823120373]
Robot Arm 2 end: [6.16176330485946;1.79430314312393;0.193301030385119]
Robot Arm 1 togoal: 20.0517072666674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000907e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.720414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.25329952352475;-5.80913239144144;-7.15559951890951]
Robot Arm 1 end: [-6.49581741848162;-6.85156907024362;0.569223463765984]
Robot Arm 1 togoal: 23.6007044599944
Mobile Platform 2 Pose: [4.26409557712944;5.99503472226295;2.31916430355644]
Robot Arm 2 end: [6.21005032263732;1.67395117569345;0.255281256149354]
Robot Arm 1 togoal: 20.0162162121976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000330e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.369918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.30571968498569;-5.77988881874546;-7.15557672703445]
Robot Arm 1 end: [-6.48617408044028;-6.88179349867373;0.554685154542438]
Robot Arm 1 togoal: 23.6161523522143
Mobile Platform 2 Pose: [4.11680244498291;6.07704682307139;2.21197322338395]
Robot Arm 2 end: [6.1886914494506;1.57808085083405;0.197626299524481]
Robot Arm 1 togoal: 19.9468545135587

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000139e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.36839447774178;-5.74584668996033;-7.15133783264865]
Robot Arm 1 end: [-6.47466652083279;-6.90907193652033;0.552172792390087]
Robot Arm 1 togoal: 23.6277374474424
Mobile Platform 2 Pose: [4.2565274411581;6.02948719733721;2.1014960033682]
Robot Arm 2 end: [6.24801942462702;1.4483833906398;0.256127780252134]
Robot Arm 1 togoal: 19.9164133120003

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998128e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.41410529876551;-5.72137946944116;-7.14860027049122]
Robot Arm 1 end: [-6.47972982326572;-6.95266250842598;0.571757304641092]
Robot Arm 1 togoal: 23.6616847005262
Mobile Platform 2 Pose: [4.09542361764956;6.12002021307969;1.99145254728831]
Robot Arm 2 end: [6.23673632794343;1.35088478158538;0.200150243160063]
Robot Arm 1 togoal: 19.8549448749708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.46369651816;-5.69336398565411;-7.1496409164855]
Robot Arm 1 end: [-6.47554483343985;-6.99557890106963;0.58444847814374]
Robot Arm 1 togoal: 23.6890356176494
Mobile Platform 2 Pose: [4.24101427284671;6.0728945345059;1.88241916818087]
Robot Arm 2 end: [6.29449071094026;1.21846527785587;0.25660898352693]
Robot Arm 1 togoal: 19.8232962789218

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989813e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.51900549412265;-5.6627011693587;-7.14985142323297]
Robot Arm 1 end: [-6.46491703223398;-7.04395766745312;0.585365225050869]
Robot Arm 1 togoal: 23.7163529717446
Mobile Platform 2 Pose: [4.07211418086623;6.16774150054256;1.75656875690883]
Robot Arm 2 end: [6.29094176453182;1.10503220393998;0.204526354420596]
Robot Arm 1 togoal: 19.7597982450585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.57152472216281;-5.63402828706854;-7.14938014652095]
Robot Arm 1 end: [-6.44890215549528;-7.09040445937934;0.571853221785228]
Robot Arm 1 togoal: 23.7391909363572
Mobile Platform 2 Pose: [4.25032313208011;6.10690128695728;1.64626023982056]
Robot Arm 2 end: [6.33053202632118;0.964771825303051;0.257774612749904]
Robot Arm 1 togoal: 19.710612264023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983547e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.61850921984817;-5.60849952407982;-7.14868854826453]
Robot Arm 1 end: [-6.42368286209464;-7.13620392907006;0.529990747942227]
Robot Arm 1 togoal: 23.7564505596059
Mobile Platform 2 Pose: [4.1008483784308;6.18747611195269;1.46451656780675]
Robot Arm 2 end: [6.31858864017078;0.789019218385221;0.212126925530592]
Robot Arm 1 togoal: 19.6064382187808

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986662e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.66299833605428;-5.58437659997768;-7.14759659606852]
Robot Arm 1 end: [-6.376939215204;-7.13095911831665;0.449098966165804]
Robot Arm 1 togoal: 23.7238881762298
Mobile Platform 2 Pose: [4.42263001108411;6.05116914283086;1.36662986178206]
Robot Arm 2 end: [6.27062630647689;0.654986926835125;0.259502376215729]
Robot Arm 1 togoal: 19.4898281950062

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990682e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.71926303928273;-5.5542932452443;-7.14545701159742]
Robot Arm 1 end: [-6.30452967288915;-7.10425293252159;0.356093072538258]
Robot Arm 1 togoal: 23.6590398026514
Mobile Platform 2 Pose: [4.89488751382338;5.8547516687715;1.21196837373352]
Robot Arm 2 end: [6.26525101093567;0.517741821713214;0.362907284023278]
Robot Arm 1 togoal: 19.4041867187408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.287202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.79522755531483;-5.51395603448046;-7.14235175451485]
Robot Arm 1 end: [-6.20737447864158;-7.15574551743879;0.258202939664248]
Robot Arm 1 togoal: 23.6345773849594
Mobile Platform 2 Pose: [4.24599616945362;6.1419746948881;1.05308238645117]
Robot Arm 2 end: [6.36290076318387;0.347106263057098;0.249551046081581]
Robot Arm 1 togoal: 19.4016419884302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991342e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.980204e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.8910581142816;-5.46229522730433;-7.13745424239528]
Robot Arm 1 end: [-6.11016775293847;-7.20298739324152;0.193508331098691]
Robot Arm 1 togoal: 23.605984715975
Mobile Platform 2 Pose: [4.52813013872093;6.01827378453408;1.01864378665525]
Robot Arm 2 end: [6.41675765235935;0.322514017216262;0.348869002589151]
Robot Arm 1 togoal: 19.4277666891235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000203e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.385034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-5.00025406384733;-5.40321337910801;-7.1302737088477]
Robot Arm 1 end: [-6.06147114978013;-7.16314305162376;0.20060063431587]
Robot Arm 1 togoal: 23.5433172979217
Mobile Platform 2 Pose: [4.99439935443476;5.82098032998678;0.905266112338944]
Robot Arm 2 end: [6.47956292040897;0.347316968195083;0.537324297641595]
Robot Arm 1 togoal: 19.4835977266886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.390642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-5.08676049965757;-5.3583394814442;-7.12528440602646]
Robot Arm 1 end: [-5.9908136341709;-7.08635629343011;0.174962607806411]
Robot Arm 1 togoal: 23.4405953151794
Mobile Platform 2 Pose: [4.65881726434332;5.97673274496813;0.867965968932642]
Robot Arm 2 end: [6.45329385831792;0.190200509393044;0.450792229257779]
Robot Arm 1 togoal: 19.3828896164208

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.222410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-5.16639267729733;-5.31778990846422;-7.1209766920271]
Robot Arm 1 end: [-5.92893047480814;-6.99133833240132;0.178922646234124]
Robot Arm 1 togoal: 23.3289000173796
Mobile Platform 2 Pose: [4.33076756091358;6.11945633510581;0.85480706665916]
Robot Arm 2 end: [6.5056721954771;0.126739484894413;0.504539981455358]
Robot Arm 1 togoal: 19.3912675756064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.223242e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-5.26131398099646;-5.26995972770769;-7.1171988989075]
Robot Arm 1 end: [-5.86638680714928;-6.8728556743617;0.227605799918458]
Robot Arm 1 togoal: 23.1971627031998
Mobile Platform 2 Pose: [4.08732808039969;6.22562302627506;0.850276460816575]
Robot Arm 2 end: [6.49029001508984;0.0726330957613257;0.645870673190997]
Robot Arm 1 togoal: 19.3430466473676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.145758e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-5.35575171955045;-5.22327139744858;-7.1129031443012]
Robot Arm 1 end: [-5.76590879222527;-6.76673377171132;0.239489090460648]
Robot Arm 1 togoal: 23.050585928073
Mobile Platform 2 Pose: [3.84047579344634;6.33352799697578;0.84137311849297]
Robot Arm 2 end: [6.44501355171939;0.0505848076130118;0.798523472748479]
Robot Arm 1 togoal: 19.2866455876517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942733e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-5.58584788761973;-5.11451720447571;-7.10002732026658]
Robot Arm 1 end: [-5.71422839460742;-6.81389927663771;0.267600561064283]
Robot Arm 1 togoal: 23.0481060626753
Mobile Platform 2 Pose: [3.51679637603653;6.46921351190602;0.828555897060781]
Robot Arm 2 end: [6.53553050075244;0.0721959457023886;0.947206857023279]
Robot Arm 1 togoal: 19.3701352261894

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-5.62240530998975;-5.09271475987706;-7.09989342810848]
Robot Arm 1 end: [-5.62242810239975;-6.73563363760035;0.238811506666475]
Robot Arm 1 togoal: 22.9300174530912
Mobile Platform 2 Pose: [3.17992445919772;6.60375958305358;0.799207776034596]
Robot Arm 2 end: [6.77976519627781;0.124670562791176;1.08697118476101]
Robot Arm 1 togoal: 19.6024961435967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993035e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-5.83725662889585;-5.00975464132567;-7.0713049401275]
Robot Arm 1 end: [-5.47739239736825;-6.64275471388622;0.22637137162842]
Robot Arm 1 togoal: 22.7641609371857
Mobile Platform 2 Pose: [2.88701527264808;6.73320987628292;0.735006360278613]
Robot Arm 2 end: [6.95555011705258;0.115866466133697;1.14853997129446]
Robot Arm 1 togoal: 19.7474089592917

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975676e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-6.02095606512018;-4.93880336371355;-7.039589281198]
Robot Arm 1 end: [-5.31518597795315;-6.51352395087181;0.241389320260138]
Robot Arm 1 togoal: 22.5585637465468
Mobile Platform 2 Pose: [2.7444187113102;6.81997765438637;0.663737210527767]
Robot Arm 2 end: [6.93066510198153;0.0360268827896323;1.17999033303098]
Robot Arm 1 togoal: 19.6846524983424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.912688e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-6.23707731522983;-4.85655580959881;-6.98862055511675]
Robot Arm 1 end: [-5.10614243974919;-6.35630769257058;0.294020088772925]
Robot Arm 1 togoal: 22.2986506418346
Mobile Platform 2 Pose: [2.80682314438778;6.80127342101241;0.539846891304474]
Robot Arm 2 end: [6.7718346136273;-0.0334336333670426;1.26136464553917]
Robot Arm 1 togoal: 19.5113887759086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.860298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-6.49103378931349;-4.7336977205131;-6.95095774105085]
Robot Arm 1 end: [-4.86623339791692;-6.23150230301432;0.424424329718523]
Robot Arm 1 togoal: 22.0379000963057
Mobile Platform 2 Pose: [3.07025946140977;6.65992653947137;0.423738596885921]
Robot Arm 2 end: [6.61548498676456;-0.0461216356885786;1.35738102069231]
Robot Arm 1 togoal: 19.3695845853053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.625899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-6.72061638710025;-4.6144771881656;-6.92819334632727]
Robot Arm 1 end: [-4.61658490964473;-6.15176320334604;0.586048096394009]
Robot Arm 1 togoal: 21.8036268556374
Mobile Platform 2 Pose: [3.13128792816065;6.65077780586239;0.388583590847194]
Robot Arm 2 end: [6.46767287803747;-0.0294821301673098;1.37343480516048]
Robot Arm 1 togoal: 19.2514220798733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.422829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-6.9039289168794;-4.51525707001254;-6.91837617117934]
Robot Arm 1 end: [-4.40431706005666;-6.13239756517271;0.67612950568849]
Robot Arm 1 togoal: 21.6437613949052
Mobile Platform 2 Pose: [3.41691675466592;6.49283754164609;0.296676186344472]
Robot Arm 2 end: [6.28816507890589;-0.0480704261734013;1.41018872800675]
Robot Arm 1 togoal: 19.0881508538175

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.052607e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-7.03964596372606;-4.44088739962147;-6.91630513884146]
Robot Arm 1 end: [-4.14946391761186;-6.16699153981857;0.668443438827109]
Robot Arm 1 togoal: 21.5012975723412
Mobile Platform 2 Pose: [3.63275309698821;6.3995017803868;0.224351967489215]
Robot Arm 2 end: [6.11924729716046;-0.0459296320323406;1.4172235044333]
Robot Arm 1 togoal: 18.9453007333877

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.232437e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-7.11142826715076;-4.40161459157235;-6.91774440951128]
Robot Arm 1 end: [-4.03531517292143;-6.19249097640038;0.689247225198266]
Robot Arm 1 togoal: 21.4447813862123
Mobile Platform 2 Pose: [3.9296831945542;6.23390609533575;0.160297514422445]
Robot Arm 2 end: [5.94148083393116;-0.0603539827401617;1.43072666866011]
Robot Arm 1 togoal: 18.7865782781585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.937928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-7.15073739960618;-4.37973351714645;-6.91808036559024]
Robot Arm 1 end: [-4.07014136446119;-6.1499831699247;0.692384562404274]
Robot Arm 1 togoal: 21.4354248371287
Mobile Platform 2 Pose: [4.20476502328212;6.08838841488596;0.100210963653819]
Robot Arm 2 end: [5.75660509713646;-0.0507104356727676;1.43187327337617]
Robot Arm 1 togoal: 18.6350988887995
Step 105:
Mobile Platform 1 Pose: [-7.18287286490442;-4.36121241934271;-6.9180230712699]
Robot Arm 1 end: [-4.17088312708405;-6.11691837232473;0.808072811651314]
Robot Arm 1 togoal: 21.4727094610238
Mobile Platform 2 Pose: [4.52520363282406;5.87830407844553;0.0651560360892359]
Robot Arm 2 end: [5.53193355230374;-0.0234437992760046;1.43559474481673]
Robot Arm 1 togoal: 18.4602255554204
Step 106:
Mobile Platform 1 Pose: [-7.23706667345695;-4.33126389020918;-6.91862831841575]
Robot Arm 1 end: [-4.24538372400519;-6.12375575660531;0.925431481299146]
Robot Arm 1 togoal: 21.5235024799237
Mobile Platform 2 Pose: [4.84815422208688;5.64199962793967;0.0591398467943964]
Robot Arm 2 end: [5.28837523908118;0.0287854349020128;1.43712006193422]
Robot Arm 1 togoal: 18.2843646483618
Step 107:
Mobile Platform 1 Pose: [-7.30673361943008;-4.29289462157219;-6.91891752153583]
Robot Arm 1 end: [-4.30327723025163;-6.16076769613204;1.03658469950112]
Robot Arm 1 togoal: 21.5867739654895
Mobile Platform 2 Pose: [5.1296692193961;5.40490616818194;0.0944999706414031]
Robot Arm 2 end: [5.04982837235764;0.117672282476408;1.43695228182119]
Robot Arm 1 togoal: 18.1348198356627
Step 108:
Mobile Platform 1 Pose: [-7.38980555092137;-4.24705127309967;-6.91829150606403]
Robot Arm 1 end: [-4.34946083899075;-6.22217365528727;1.1360648185276]
Robot Arm 1 togoal: 21.6613508250515
Mobile Platform 2 Pose: [5.28130486000966;5.2385506968708;0.166852763990732]
Robot Arm 2 end: [4.88483344711041;0.158738782645796;1.43831960197603]
Robot Arm 1 togoal: 18.0212395925049
Step 109:
Mobile Platform 1 Pose: [-7.48652771637609;-4.19358175804939;-6.91606790938124]
Robot Arm 1 end: [-4.37862343931827;-6.28271079753669;1.19906629111776]
Robot Arm 1 togoal: 21.7249884749991
Mobile Platform 2 Pose: [5.318360472535;5.13750590999734;0.246118919927433]
Robot Arm 2 end: [4.80432023758567;0.171616601606538;1.44494434365806]
Robot Arm 1 togoal: 17.9620562979278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.375730e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-7.59379461088857;-4.13452881652866;-6.91183019631831]
Robot Arm 1 end: [-4.38860460546395;-6.19332177387678;1.0593554510617]
Robot Arm 1 togoal: 21.6672270550307
Mobile Platform 2 Pose: [5.31087089164034;5.04052769018093;0.332558781838236]
Robot Arm 2 end: [4.7450538877692;0.212001190138764;1.45566282520477]
Robot Arm 1 togoal: 17.9361604421633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.817768e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-7.67731376367211;-4.08938278121557;-6.90899592707004]
Robot Arm 1 end: [-4.35023559096616;-6.17308659701061;1.03416859770024]
Robot Arm 1 togoal: 21.6271818877287
Mobile Platform 2 Pose: [5.26651564039782;4.96458557068751;0.416754585970287]
Robot Arm 2 end: [4.71995577900886;0.263726618212542;1.46846935155049]
Robot Arm 1 togoal: 17.9450255418711

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965309e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-7.75317991594394;-4.04846451116918;-6.90679746637029]
Robot Arm 1 end: [-4.27883027231377;-6.21678618334631;1.13199473478366]
Robot Arm 1 togoal: 21.6106564721732
Mobile Platform 2 Pose: [5.23860315383951;4.86271123784272;0.507927957033197]
Robot Arm 2 end: [4.65916355883925;0.334357814448446;1.47602306564245]
Robot Arm 1 togoal: 17.9357757180022

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955197e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.204083e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-7.8367802212495;-4.00344928628172;-6.90390552276323]
Robot Arm 1 end: [-4.2172338990066;-6.26336962496812;1.20398306526191]
Robot Arm 1 togoal: 21.6038753707364
Mobile Platform 2 Pose: [5.22132335209559;4.75601498107468;0.600551998797119]
Robot Arm 2 end: [4.60286871484711;0.482153301177178;1.49396767480009]
Robot Arm 1 togoal: 17.9755401387287

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.940612e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.824156e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-7.92739127137894;-3.95481350816852;-6.9003004950998]
Robot Arm 1 end: [-4.16818892563887;-6.30322444408592;1.24503081128211]
Robot Arm 1 togoal: 21.6011310745516
Mobile Platform 2 Pose: [5.29078475687134;4.69247657903774;0.645775023945609]
Robot Arm 2 end: [4.56669031671096;0.465326702501524;1.48214287852139]
Robot Arm 1 togoal: 17.9363608251212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.839355e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.605146e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-8.02380659476471;-3.90343618353846;-6.89495836111867]
Robot Arm 1 end: [-4.07755626004088;-6.34533930432195;1.28561919000593]
Robot Arm 1 togoal: 21.5732035487023
Mobile Platform 2 Pose: [5.30990388206803;4.66812398653168;0.679541069166989]
Robot Arm 2 end: [4.6023160117505;0.337927965191399;1.42373661688825]
Robot Arm 1 togoal: 17.891613892721

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.831423e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-8.13101198862778;-3.8468112793985;-6.88491229583558]
Robot Arm 1 end: [-4.01861218863239;-6.42997319424786;1.33415716512944]
Robot Arm 1 togoal: 21.5986207222572
Mobile Platform 2 Pose: [5.25668075015133;4.64819245887201;0.761711154103692]
Robot Arm 2 end: [4.67792698077146;0.293639477809746;1.3343082491893]
Robot Arm 1 togoal: 17.9286198008046

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.907683e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-8.24143110051352;-3.79077709399069;-6.87032136133096]
Robot Arm 1 end: [-3.97266236386526;-6.48124639462686;1.3417128862705]
Robot Arm 1 togoal: 21.6078439489952
Mobile Platform 2 Pose: [5.22314753983184;4.63852944337342;0.84600527208651]
Robot Arm 2 end: [4.72300448466978;0.287359502463693;1.23650686955823]
Robot Arm 1 togoal: 17.9632280765403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949481e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-8.30532057394009;-3.76251838741657;-6.85281528274828]
Robot Arm 1 end: [-3.8510420737509;-6.37985677779303;1.32305562983352]
Robot Arm 1 togoal: 21.4520455100093
Mobile Platform 2 Pose: [5.24090759546208;4.63279802931503;0.90471057939548]
Robot Arm 2 end: [4.85874193158889;0.289359936586186;1.10634893039425]
Robot Arm 1 togoal: 18.0783039913513

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-8.39141898264499;-3.72065712759496;-6.8375611087696]
Robot Arm 1 end: [-3.7308684108179;-6.39384633590125;1.34566921171363]
Robot Arm 1 togoal: 21.3851744413194
Mobile Platform 2 Pose: [5.38723888138787;4.54157326086143;0.99987371965802]
Robot Arm 2 end: [4.84797188294834;0.376000092154854;1.01400919277439]
Robot Arm 1 togoal: 18.1212771798191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958186e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-8.48183161498566;-3.68239760823516;-6.81161264951618]
Robot Arm 1 end: [-3.63978261326837;-6.42382744503354;1.37046952862878]
Robot Arm 1 togoal: 21.3496706556393
Mobile Platform 2 Pose: [5.5620367970022;4.44610645096984;1.05877700969624]
Robot Arm 2 end: [4.79417119133908;0.429428697775581;0.92815985969642]
Robot Arm 1 togoal: 18.110547855672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-8.54433103728683;-3.6697221989625;-6.77977095884258]
Robot Arm 1 end: [-3.70303277443946;-6.39016729253459;1.35547205479158]
Robot Arm 1 togoal: 21.3644117774227
Mobile Platform 2 Pose: [5.73682376925127;4.35248844951557;1.11214774295163]
Robot Arm 2 end: [4.72060614216003;0.478923628975703;0.853044740361704]
Robot Arm 1 togoal: 18.0817656727263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982605e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-8.59025831702364;-3.64097218933824;-6.78458394506057]
Robot Arm 1 end: [-3.79477752892181;-6.34027225847816;1.2915304532803]
Robot Arm 1 togoal: 21.3858140089759
Mobile Platform 2 Pose: [5.90701245084448;4.2634496571799;1.16678845447228]
Robot Arm 2 end: [4.63697793663838;0.517097219963366;0.782669530963071]
Robot Arm 1 togoal: 18.0387247036288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945519e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-8.6289948735441;-3.61790583363092;-6.78777053519885]
Robot Arm 1 end: [-3.88937314291685;-6.28552807376415;1.20182928264939]
Robot Arm 1 togoal: 21.4064238230339
Mobile Platform 2 Pose: [6.0712458519735;4.18393993740377;1.23150642839073]
Robot Arm 2 end: [4.56651656231558;0.574002100895515;0.698370527496306]
Robot Arm 1 togoal: 18.0185916330602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-8.59876062514636;-3.63217633708478;-6.78780406710627]
Robot Arm 1 end: [-4.02566211133653;-6.25840698368346;1.09578639250126]
Robot Arm 1 togoal: 21.4764010467413
Mobile Platform 2 Pose: [6.21427147812015;4.12616514087414;1.30347758355875]
Robot Arm 2 end: [4.52203499652752;0.653280439540749;0.589034085431987]
Robot Arm 1 togoal: 18.0347011350282

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-8.64276582188138;-3.6104179158743;-6.78674161276134]
Robot Arm 1 end: [-4.12593719946877;-6.27101868693637;0.976793019048922]
Robot Arm 1 togoal: 21.554217965681
Mobile Platform 2 Pose: [6.33608242789103;4.09263967103073;1.38539009991096]
Robot Arm 2 end: [4.50106214979435;0.766443265052763;0.460455747406665]
Robot Arm 1 togoal: 18.0920385297928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960339e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-8.6128718246391;-3.6252330887805;-6.78606064514601]
Robot Arm 1 end: [-4.2546918059981;-6.33547568603394;0.850506076303794]
Robot Arm 1 togoal: 21.6908636636421
Mobile Platform 2 Pose: [6.43838432289685;4.08435489940734;1.48079153163151]
Robot Arm 2 end: [4.49281801902578;0.920409169753114;0.322353327316609]
Robot Arm 1 togoal: 18.1860898604775

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959558e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-8.57789405468945;-3.64095471818122;-6.78671770247999]
Robot Arm 1 end: [-4.36053396524049;-6.4535764568368;0.72491618605988]
Robot Arm 1 togoal: 21.8535778175249
Mobile Platform 2 Pose: [6.51440492695726;4.10536410197278;1.60202821390811]
Robot Arm 2 end: [4.52663635724792;1.06760001163852;0.294502209666127]
Robot Arm 1 togoal: 18.303530552957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-8.59433157392292;-3.62938676497013;-6.7883298448329]
Robot Arm 1 end: [-4.3552509517581;-6.47961164478895;0.606398263539332]
Robot Arm 1 togoal: 21.8743531604332
Mobile Platform 2 Pose: [6.46906198614038;4.2267037489591;1.6967481817844]
Robot Arm 2 end: [4.65807629736702;1.21107476747295;0.24722063562714]
Robot Arm 1 togoal: 18.4978353341791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.117317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-8.63101781296928;-3.61043893286706;-6.78757394498053]
Robot Arm 1 end: [-4.30785792803803;-6.46765993692547;0.477242026706366]
Robot Arm 1 togoal: 21.8400561582143
Mobile Platform 2 Pose: [6.47173324633709;4.32908735409727;1.82668026100827]
Robot Arm 2 end: [4.77120418876053;1.43191798957009;0.176940327865558]
Robot Arm 1 togoal: 18.7265511642847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977105e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.920944e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-8.64624807529391;-3.60626075404161;-6.78275459066522]
Robot Arm 1 end: [-4.32198283013517;-6.56237352896433;0.39963685172668]
Robot Arm 1 togoal: 21.924617771125
Mobile Platform 2 Pose: [6.68293656527724;4.2019791674341;2.04195571748822]
Robot Arm 2 end: [4.63327705864934;1.55465796214808;0.248141260082451]
Robot Arm 1 togoal: 18.6885817404427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985047e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.684671e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-8.64938692394594;-3.60445125211774;-6.78149678510183]
Robot Arm 1 end: [-4.30147678085962;-6.52647422787324;0.394513041040403]
Robot Arm 1 togoal: 21.8843679686376
Mobile Platform 2 Pose: [6.51857126928693;4.41717648029041;2.09869797159609]
Robot Arm 2 end: [4.8297137805017;1.6757843922249;0.182780195583568]
Robot Arm 1 togoal: 18.9218176777565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000881e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.761861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-8.73728715778224;-3.55959313289428;-6.78075566692183]
Robot Arm 1 end: [-4.28370615739735;-6.65770748440601;0.389034296831113]
Robot Arm 1 togoal: 21.9723650656923
Mobile Platform 2 Pose: [6.52681058474109;4.47885847974415;2.22964906868073]
Robot Arm 2 end: [4.78975210112694;1.69214371675066;0.202831856363661]
Robot Arm 1 togoal: 18.8992211257723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.411896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-8.74268850002174;-3.53588946515527;-6.78747680967801]
Robot Arm 1 end: [-4.24332638143813;-6.72919203815001;0.356285287042387]
Robot Arm 1 togoal: 22.0021538268428
Mobile Platform 2 Pose: [6.61982520507708;4.48396667089122;2.1483991409922]
Robot Arm 2 end: [4.81100812313657;1.64916847646802;0.172016319319979]
Robot Arm 1 togoal: 18.8915019198447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.285528e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-8.76100170975165;-3.51531519954397;-6.79129466526894]
Robot Arm 1 end: [-4.21655091028225;-6.79473423578579;0.385885428269778]
Robot Arm 1 togoal: 22.0332000155737
Mobile Platform 2 Pose: [6.76518942740844;4.48340742833819;2.03334860573176]
Robot Arm 2 end: [4.75523930341137;1.44515565622991;0.207835702428463]
Robot Arm 1 togoal: 18.719857781743

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007910e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.953602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-8.7879251371991;-3.4893321319232;-6.79469895417031]
Robot Arm 1 end: [-4.19499366472919;-6.87824971950494;0.488839303448756]
Robot Arm 1 togoal: 22.0780097481448
Mobile Platform 2 Pose: [7.03971787034953;4.36300325386561;1.95799719169019]
Robot Arm 2 end: [4.51210847970419;1.35994380586267;0.141216765758475]
Robot Arm 1 togoal: 18.4811638587118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985431e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-8.78881097543845;-3.47028128990524;-6.79695270670263]
Robot Arm 1 end: [-4.14072785493254;-6.91563276410392;0.43624431481257]
Robot Arm 1 togoal: 22.0743179050642
Mobile Platform 2 Pose: [7.29248106037024;4.23342024317138;1.91036341201947]
Robot Arm 2 end: [4.26569563873138;1.27407814709188;0.155896959317347]
Robot Arm 1 togoal: 18.2339906886928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006227e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000307e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-8.82838651627509;-3.44195700852384;-6.79886195399021]
Robot Arm 1 end: [-4.07427682797458;-7.01024308868603;0.49587722958598]
Robot Arm 1 togoal: 22.1016857922891
Mobile Platform 2 Pose: [7.51424418194776;4.11247948232992;1.87724464122648]
Robot Arm 2 end: [4.01777704213332;1.22964131539619;0.173590338184593]
Robot Arm 1 togoal: 18.0116193073295

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001619e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-8.846095741589;-3.42129777738083;-6.7998177456835]
Robot Arm 1 end: [-4.00707737233799;-7.02300035437525;0.465075614307137]
Robot Arm 1 togoal: 22.0702454424056
Mobile Platform 2 Pose: [7.72147469743526;3.99394320894755;1.85028108664889]
Robot Arm 2 end: [3.78107370230359;1.23356709215208;0.167228606638569]
Robot Arm 1 togoal: 17.8309763890733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996195e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-8.88524456203775;-3.39358224779712;-6.80009745157339]
Robot Arm 1 end: [-3.93562329657345;-6.97968121599303;0.42810101230193]
Robot Arm 1 togoal: 21.9933080517247
Mobile Platform 2 Pose: [7.9120087387989;3.88081130710528;1.83079185571834]
Robot Arm 2 end: [3.57518141754793;1.31180125802669;0.106012377745568]
Robot Arm 1 togoal: 17.7269452583226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-8.90917238397304;-3.37571781861215;-6.80099325484224]
Robot Arm 1 end: [-3.87383327254222;-6.93019383898153;0.381171759843036]
Robot Arm 1 togoal: 21.9183466875777
Mobile Platform 2 Pose: [8.20569544564271;3.68993177966065;1.80138140142748]
Robot Arm 2 end: [3.33495568560407;1.41753109992653;0.00668427140944944]
Robot Arm 1 togoal: 17.6202784223369

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024456e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-8.91748976289694;-3.36614414505392;-6.80299310718856]
Robot Arm 1 end: [-3.82239665711747;-6.87046264478415;0.317851560542538]
Robot Arm 1 togoal: 21.8430475292052
Mobile Platform 2 Pose: [8.57484861475328;3.30636612284304;1.7563773177103]
Robot Arm 2 end: [2.9348743132509;1.48702550684105;-0.0714899199282613]
Robot Arm 1 togoal: 17.3723559898906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.417108e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-8.91742124695256;-3.36131345496972;-6.80584400754451]
Robot Arm 1 end: [-3.76931083441277;-6.82190149435961;0.246527710495455]
Robot Arm 1 togoal: 21.7760090548597
Mobile Platform 2 Pose: [8.41453975386708;3.14103555464227;1.75830237970173]
Robot Arm 2 end: [2.81588157291399;1.3299529206093;-0.0097202217707541]
Robot Arm 1 togoal: 17.1743342832122

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.046542e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-8.91621677262577;-3.35763703220376;-6.80902208255158]
Robot Arm 1 end: [-3.70627935640423;-6.74163551045278;0.174288865549853]
Robot Arm 1 togoal: 21.6785166522135
Mobile Platform 2 Pose: [8.60685611517867;2.99353819907568;1.73829354141305]
Robot Arm 2 end: [2.64658049161885;1.34495642605102;0.0295045392444575]
Robot Arm 1 togoal: 17.0548700713153

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.580393e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.059227e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-8.91457952240865;-3.355172028216;-6.81253669752109]
Robot Arm 1 end: [-3.63424008083801;-6.77246084423999;0.0922144736388843]
Robot Arm 1 togoal: 21.6621645602149
Mobile Platform 2 Pose: [8.77963086582513;2.87614946747994;1.72529592380307]
Robot Arm 2 end: [2.49145915277416;1.51173966607478;0.0168440078244421]
Robot Arm 1 togoal: 17.0534215673484

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.421117e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.676900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-8.91509270459825;-3.35125208464333;-6.81795238361907]
Robot Arm 1 end: [-3.55777234220308;-6.83376764410396;0.0230692119652687]
Robot Arm 1 togoal: 21.6663961152849
Mobile Platform 2 Pose: [8.65478544844006;2.88120630498181;1.73056530116366]
Robot Arm 2 end: [2.48926213471423;1.42567444135684;0.0579423461383099]
Robot Arm 1 togoal: 16.9902396336251

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.575379e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.325310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-8.93493800520413;-3.33581989879076;-6.82604474046626]
Robot Arm 1 end: [-3.52024279288482;-6.69407889321213;-0.00481104351123723]
Robot Arm 1 togoal: 21.5364187972802
Mobile Platform 2 Pose: [8.46313587492126;2.89330593196511;1.72961429282628]
Robot Arm 2 end: [2.45988927031257;1.55973458391728;0.0822924635309126]
Robot Arm 1 togoal: 17.0571446512761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-8.95861478606909;-3.3190022204021;-6.83393777400082]
Robot Arm 1 end: [-3.50477399358949;-6.53394127265237;0.0280535059531254]
Robot Arm 1 togoal: 21.4004444780835
Mobile Platform 2 Pose: [8.54310794993538;2.80378843421679;1.72076131559512]
Robot Arm 2 end: [2.42003548791676;1.54902139126486;0.097382959081643]
Robot Arm 1 togoal: 17.0195141557037

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993292e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-8.96647349910765;-3.31289699001296;-6.84045575288943]
Robot Arm 1 end: [-3.47601676205025;-6.46892078274956;0.0429004040160897]
Robot Arm 1 togoal: 21.3310374322298
Mobile Platform 2 Pose: [8.646638367529;2.73447560573475;1.71465862103593]
Robot Arm 2 end: [2.41145769990997;1.40031063919246;0.0918722113540199]
Robot Arm 1 togoal: 16.9131242277416

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.264112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-8.96788081063925;-3.31144324947488;-6.84784516422131]
Robot Arm 1 end: [-3.44604045178892;-6.41243766080655;0.0682864307132232]
Robot Arm 1 togoal: 21.266746119597
Mobile Platform 2 Pose: [8.70772760833064;2.65242093816851;1.70162451503707]
Robot Arm 2 end: [2.43157159239334;1.36457237585609;0.0794489926176707]
Robot Arm 1 togoal: 16.9048971262696

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-8.96507517581711;-3.31318244873758;-6.85832974546067]
Robot Arm 1 end: [-3.418144645141;-6.36728436620143;0.104401036258363]
Robot Arm 1 togoal: 21.2118211350676
Mobile Platform 2 Pose: [8.8166259102448;2.56809110986754;1.67957305205842]
Robot Arm 2 end: [2.44617194368127;1.24049233273285;0.0907402153521912]
Robot Arm 1 togoal: 16.8315628206499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.827056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.698148e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-8.98185878299867;-3.3044044975375;-6.87979213661156]
Robot Arm 1 end: [-3.37789019719999;-6.36045194959675;0.0991408479863624]
Robot Arm 1 togoal: 21.1814545669453
Mobile Platform 2 Pose: [8.79411263651331;2.50893030356451;1.64629899968172]
Robot Arm 2 end: [2.45692439471709;1.13781279193809;0.112989912643582]
Robot Arm 1 togoal: 16.7692674726732

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.984072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.052273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-9.24421958877289;-3.15813879311623;-6.96074996962186]
Robot Arm 1 end: [-3.16577802483274;-6.41615810421216;0.137340686656727]
Robot Arm 1 togoal: 21.0889182035746
Mobile Platform 2 Pose: [8.54365403470513;2.54888989053486;1.61300640009809]
Robot Arm 2 end: [2.34618003260069;1.08212702930957;0.146029061508189]
Robot Arm 1 togoal: 16.6472894482207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.932258e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-9.45191552434732;-3.03936739275547;-6.98818490647372]
Robot Arm 1 end: [-2.98449812183653;-6.39125409114778;0.189494807661209]
Robot Arm 1 togoal: 20.9533603088805
Mobile Platform 2 Pose: [8.31003447922065;2.50655052372725;1.56929193147736]
Robot Arm 2 end: [2.14618004328996;1.04234444033453;0.224563060604905]
Robot Arm 1 togoal: 16.4664449448213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.848371e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-9.67892680678669;-2.91740848037678;-7.0567513354122]
Robot Arm 1 end: [-2.85395089193491;-6.4219805574807;0.237315132235506]
Robot Arm 1 togoal: 20.8938590452381
Mobile Platform 2 Pose: [8.40026708916866;2.21003816533668;1.50714100919275]
Robot Arm 2 end: [1.96476768848443;0.96601218910011;0.162912323065535]
Robot Arm 1 togoal: 16.2866046587106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.677594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-9.76065698353035;-2.875803231817;-7.12382311762302]
Robot Arm 1 end: [-2.81383658156823;-6.49450515628029;0.214312672424643]
Robot Arm 1 togoal: 20.9277456994713
Mobile Platform 2 Pose: [8.70440590928323;1.90505922195625;1.39589660204669]
Robot Arm 2 end: [1.77636841822174;0.784246561552593;0.228852205523761]
Robot Arm 1 togoal: 16.0203768040836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-9.82361099159204;-2.84870152677264;-7.21269525718525]
Robot Arm 1 end: [-2.76633086617815;-6.57277947055177;0.230506975228538]
Robot Arm 1 togoal: 20.9595227858665
Mobile Platform 2 Pose: [8.31008718370716;1.92958791674767;1.35577383327392]
Robot Arm 2 end: [1.65456293487942;0.735768704606893;0.27531119925967]
Robot Arm 1 togoal: 15.894568489587

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999985e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-9.96230788287133;-2.78858929789087;-7.34356820622552]
Robot Arm 1 end: [-2.68125348180481;-6.66961588345826;0.293293035941545]
Robot Arm 1 togoal: 20.9808852997539
Mobile Platform 2 Pose: [7.96353980476054;2.03400289301955;1.355948733993]
Robot Arm 2 end: [1.64345068256305;0.809686630447223;0.218534064259146]
Robot Arm 1 togoal: 15.9410064412355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.927368e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-9.71237392084777;-2.91885155051753;-7.27577400911866]
Robot Arm 1 end: [-2.93841281813051;-6.63041878249964;0.231112861020055]
Robot Arm 1 togoal: 21.1101199419848
Mobile Platform 2 Pose: [7.59203466662097;2.14514309702939;1.34880330357757]
Robot Arm 2 end: [1.62481327060132;0.835384575969718;0.275932040164884]
Robot Arm 1 togoal: 15.9402646373929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.960615e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-9.81836238668551;-2.90720927847883;-7.52919067384137]
Robot Arm 1 end: [-2.79016872165598;-6.84845074119407;0.277284720206612]
Robot Arm 1 togoal: 21.1897507301068
Mobile Platform 2 Pose: [7.61010767138926;1.94241469130966;1.20251183590304]
Robot Arm 2 end: [1.50986976052302;0.595066299319182;0.369137840617682]
Robot Arm 1 togoal: 15.6862780233358

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001445e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-9.53277983598396;-3.02984735561974;-7.41780482328147]
Robot Arm 1 end: [-3.03567280179445;-6.76860984376117;0.207885713970513]
Robot Arm 1 togoal: 21.2800213960396
Mobile Platform 2 Pose: [7.28855212924484;2.08770847989114;1.22171488867173]
Robot Arm 2 end: [1.54606321823002;0.683402980513349;0.350925135100442]
Robot Arm 1 togoal: 15.7738945816458

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952033e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-9.20640481514242;-3.15231241488947;-7.19505281040278]
Robot Arm 1 end: [-3.09427595633233;-6.55683024227624;0.166698122403701]
Robot Arm 1 togoal: 21.1523715944051
Mobile Platform 2 Pose: [7.07297652429371;2.18371321508379;1.23252305667468]
Robot Arm 2 end: [1.50764533406211;0.771569053897329;0.453437765442663]
Robot Arm 1 togoal: 15.7984901222266

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.863906e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-8.97380115599857;-3.25564242728962;-7.12703081093655]
Robot Arm 1 end: [-3.36903989243389;-6.53199951752017;0.176616629805132]
Robot Arm 1 togoal: 21.3036523239287
Mobile Platform 2 Pose: [6.84335852708035;2.28025794852703;1.23976324705649]
Robot Arm 2 end: [1.50093511905186;0.883222594458797;0.609726947689243]
Robot Arm 1 togoal: 15.8602163046109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001331e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-8.77781132952479;-3.3609790649432;-7.07052333374964]
Robot Arm 1 end: [-3.56541333726626;-6.48508004752466;0.209687868806981]
Robot Arm 1 togoal: 21.3891992063656
Mobile Platform 2 Pose: [6.52085745987037;2.39617686549564;1.24254055216015]
Robot Arm 2 end: [1.55814230943989;1.01674740447354;0.672136352852228]
Robot Arm 1 togoal: 15.9899952004586

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-8.55467874379216;-3.49036096892342;-7.02802449473822]
Robot Arm 1 end: [-3.80469441161323;-6.49500466462851;0.209321530327816]
Robot Arm 1 togoal: 21.5493681367214
Mobile Platform 2 Pose: [6.19863817157653;2.45107535666069;1.22584532682046]
Robot Arm 2 end: [1.60061639018026;1.10730334051765;0.754192886311927]
Robot Arm 1 togoal: 16.0790074718558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969488e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-8.32584363968895;-3.62779951447758;-7.00394759090438]
Robot Arm 1 end: [-4.0390559239284;-6.53392017195936;0.215836582719011]
Robot Arm 1 togoal: 21.7294514204506
Mobile Platform 2 Pose: [5.96081732058301;2.52264014209728;1.23379202353417]
Robot Arm 2 end: [1.72981134355544;1.03241011602435;0.82374708724637]
Robot Arm 1 togoal: 16.117962301355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-8.08046057730659;-3.7769080531939;-6.9929288551039]
Robot Arm 1 end: [-4.19756086796078;-6.60848895925435;0.133550989483305]
Robot Arm 1 togoal: 21.8937811916057
Mobile Platform 2 Pose: [5.98618666658168;2.42190150766945;1.23778259623259]
Robot Arm 2 end: [1.59853393352397;1.12598165697685;0.734499982437786]
Robot Arm 1 togoal: 16.091365371407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-7.68888425188671;-4.01238471966476;-6.9872743199527]
Robot Arm 1 end: [-4.49061812880041;-6.72196048083576;0.113453603041204]
Robot Arm 1 togoal: 22.1716747887241
Mobile Platform 2 Pose: [6.11384571514962;2.3189317706661;1.24395675746199]
Robot Arm 2 end: [1.54258443648308;1.2836791313713;0.924338716514016]
Robot Arm 1 togoal: 16.1526677332352

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-7.65608844017163;-3.95883966801879;-7.24219946611248]
Robot Arm 1 end: [-4.37728880339734;-6.87890780652912;0.190948040926676]
Robot Arm 1 togoal: 22.2120012671402
Mobile Platform 2 Pose: [6.29425547268411;2.15862540384382;1.26686287557309]
Robot Arm 2 end: [1.50893717565651;1.45393501350829;1.09161830691694]
Robot Arm 1 togoal: 16.2429255630023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999115e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-7.50800093256144;-4.04680121438286;-7.24494111511429]
Robot Arm 1 end: [-4.47341093018264;-6.89357720917502;0.121926316839956]
Robot Arm 1 togoal: 22.2896833097108
Mobile Platform 2 Pose: [6.21231731686456;2.03199595967335;1.35712547666407]
Robot Arm 2 end: [1.31292738556005;1.51424423473482;0.996010658737905]
Robot Arm 1 togoal: 16.1504105253247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999435e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-7.15407471959948;-4.25649122693983;-7.24396538573623]
Robot Arm 1 end: [-4.7838608669598;-6.92282333472715;0.158760257958777]
Robot Arm 1 togoal: 22.5122230991701
Mobile Platform 2 Pose: [6.37281403615529;1.83151943385296;1.41028297829048]
Robot Arm 2 end: [1.2580094910495;1.65232419986799;1.14473312304539]
Robot Arm 1 togoal: 16.2068199756178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.972834e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-6.64098711295069;-4.49338530607632;-7.32491142862385]
Robot Arm 1 end: [-5.00709680257315;-6.91002220036161;0.184917148570069]
Robot Arm 1 togoal: 22.6483315642546
Mobile Platform 2 Pose: [6.14397586009988;1.84220379216774;1.48187050418893]
Robot Arm 2 end: [1.12097289871207;1.62472480338412;0.997621200124278]
Robot Arm 1 togoal: 16.0960923933873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.925239e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-6.36225576884845;-4.66817576398404;-7.30176428270194]
Robot Arm 1 end: [-5.00635465007169;-6.92988231145834;0.0239256745374717]
Robot Arm 1 togoal: 22.6727331060796
Mobile Platform 2 Pose: [6.31070125166231;1.64262702970526;1.5306631466018]
Robot Arm 2 end: [1.0599353482034;1.74934484033083;1.14487743829712]
Robot Arm 1 togoal: 16.1403451718007

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010809e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-5.78306773400683;-5.00636970459674;-7.2255956100558]
Robot Arm 1 end: [-5.16717640105973;-6.95587089519811;-0.0935103688264282]
Robot Arm 1 togoal: 22.8068288353848
Mobile Platform 2 Pose: [6.08946515247091;1.64823011554072;1.6012146959981]
Robot Arm 2 end: [0.935724304622241;1.77424639113387;1.00170261983595]
Robot Arm 1 togoal: 16.0777054928519

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.870661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-5.39746379160619;-5.17933127841318;-6.96387215340685]
Robot Arm 1 end: [-5.21205326398974;-6.83653898390968;-0.136958678374534]
Robot Arm 1 togoal: 22.7513611997693
Mobile Platform 2 Pose: [6.26035127810086;1.45784628070894;1.64625056184444]
Robot Arm 2 end: [0.883861763716948;1.88327381645807;1.14658522216236]
Robot Arm 1 togoal: 16.1186485138947

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005741e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.044531e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-5.36973953205165;-5.18943389883864;-6.94206928265934]
Robot Arm 1 end: [-5.16893320089266;-6.65339448445681;-0.108933818988425]
Robot Arm 1 togoal: 22.5851448204868
Mobile Platform 2 Pose: [6.05471110562804;1.4581306326604;1.71821746635211]
Robot Arm 2 end: [0.736777320929308;1.99878155494781;0.903260482750111]
Robot Arm 1 togoal: 16.1130614146955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.863973e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.954269e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-5.12410408304877;-5.25480349644131;-6.72410709275471]
Robot Arm 1 end: [-4.99532762731116;-6.5133853054222;-0.0652495449700286]
Robot Arm 1 togoal: 22.3621987406125
Mobile Platform 2 Pose: [6.21532221121143;1.33121277865581;1.73867867406911]
Robot Arm 2 end: [0.753587756565756;2.10337451011062;1.05910141198042]
Robot Arm 1 togoal: 16.1970575869746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.810803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.910386e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-5.33706117723105;-5.06167567297649;-6.63028493230203]
Robot Arm 1 end: [-4.9160920717144;-6.41352867334484;-0.0946684333427998]
Robot Arm 1 togoal: 22.2374483853043
Mobile Platform 2 Pose: [6.24617724840102;1.19192530109118;1.82809896885012]
Robot Arm 2 end: [0.713251465300067;2.17949603778656;1.15309361997264]
Robot Arm 1 togoal: 16.2249571893943

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.476178e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.397068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-5.46230869324782;-4.94235157724115;-6.56486357096784]
Robot Arm 1 end: [-4.70443975793499;-6.25788972681553;-0.0297088594707137]
Robot Arm 1 togoal: 21.9759920124411
Mobile Platform 2 Pose: [6.03731643148681;1.20958812274066;1.90069655796186]
Robot Arm 2 end: [0.640583968159914;2.27467586575233;1.05325574042768]
Robot Arm 1 togoal: 16.2514146918078

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.999135e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.250526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-5.51698244698341;-4.86655771529089;-6.51719525936357]
Robot Arm 1 end: [-4.49017056253624;-6.17583133028322;0.00476463262665777]
Robot Arm 1 togoal: 21.7697389016597
Mobile Platform 2 Pose: [5.90335760135441;1.15144565493852;1.99205706662355]
Robot Arm 2 end: [0.493075826053747;2.39218752501171;0.898863832173434]
Robot Arm 1 togoal: 16.249861702725

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.525551e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-5.45139020614772;-4.78837879690262;-6.41249691378271]
Robot Arm 1 end: [-4.23493205733894;-6.13304169439574;0.0752881155746059]
Robot Arm 1 togoal: 21.5638217355583
Mobile Platform 2 Pose: [6.05108052387465;1.02075768008689;2.0270816624036]
Robot Arm 2 end: [0.544851779882553;2.5092888464887;1.05833488957683]
Robot Arm 1 togoal: 16.367353650147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010295e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-5.23672563852952;-4.78830389555185;-6.33663918992566]
Robot Arm 1 end: [-4.10996341490373;-6.14310713979853;0.211679338818232]
Robot Arm 1 togoal: 21.4803467451234
Mobile Platform 2 Pose: [5.98940900187146;0.943940926662008;2.1257170042632]
Robot Arm 2 end: [0.492209134348914;2.68679954738046;0.977172164406222]
Robot Arm 1 togoal: 16.4723126484745

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.370422e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-5.08403801920521;-4.72250407737402;-6.25684530831318]
Robot Arm 1 end: [-3.98404879934981;-6.17536779789472;0.287825046666843]
Robot Arm 1 togoal: 21.4176764539316
Mobile Platform 2 Pose: [6.12851390331196;0.793899144979022;2.19405646925465]
Robot Arm 2 end: [0.536085964320263;2.7487434406306;1.12221580300643]
Robot Arm 1 togoal: 16.5438385992592

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.331403e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-5.15913954343208;-4.49108946542735;-6.17174354215568]
Robot Arm 1 end: [-3.79256017269748;-6.03010731203289;0.21105188302493]
Robot Arm 1 togoal: 21.1876147931271
Mobile Platform 2 Pose: [5.95194403015998;0.798025498116043;2.29443176884347]
Robot Arm 2 end: [0.473735167051891;2.93071637420417;0.980024145695432]
Robot Arm 1 togoal: 16.6491804355227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.403367e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-4.98289926936613;-4.4534643402498;-6.10910164512705]
Robot Arm 1 end: [-3.71308503263239;-6.08458442604142;0.28815045906034]
Robot Arm 1 togoal: 21.1726822878268
Mobile Platform 2 Pose: [6.03876973704151;0.66602809333557;2.3889071512205]
Robot Arm 2 end: [0.496271979235044;2.97264273732382;1.12014171145136]
Robot Arm 1 togoal: 16.6919702977154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.717368e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-5.09593123119607;-4.21298784231749;-6.04072322145936]
Robot Arm 1 end: [-3.51659842254845;-5.89683825095508;0.199973836084172]
Robot Arm 1 togoal: 20.9082043566447
Mobile Platform 2 Pose: [5.82115156102679;0.732739478475106;2.45510201125393]
Robot Arm 2 end: [0.370050126198577;3.0777829345816;0.966242203428341]
Robot Arm 1 togoal: 16.6995240146766
Step 186:
Mobile Platform 1 Pose: [-4.90298176030497;-4.20600912466834;-5.99133995321845]
Robot Arm 1 end: [-3.4770883233525;-5.96331715874257;0.278584955786186]
Robot Arm 1 togoal: 20.9285211978328
Mobile Platform 2 Pose: [5.71104084488365;0.712098774505234;2.54903225187309]
Robot Arm 2 end: [0.268376772892603;3.17560402321395;0.849503474791607]
Robot Arm 1 togoal: 16.7178650105203
Step 187:
Mobile Platform 1 Pose: [-4.8946719900769;-4.07474744288498;-5.96507719419595]
Robot Arm 1 end: [-3.37959680363134;-6.04075262881658;0.231456382034432]
Robot Arm 1 togoal: 20.9280261436492
Mobile Platform 2 Pose: [5.75147082948831;0.611997881983835;2.6373509713844]
Robot Arm 2 end: [0.25945918803851;3.25491942304233;0.952998804728888]
Robot Arm 1 togoal: 16.7711662415052
Step 188:
Mobile Platform 1 Pose: [-4.94259409919826;-3.92667711311223;-5.95302113551101]
Robot Arm 1 end: [-3.23602783212964;-6.07072495854817;0.204867333941782]
Robot Arm 1 togoal: 20.8612760554305
Mobile Platform 2 Pose: [5.58716484487158;0.627947799013081;2.72362967816738]
Robot Arm 2 end: [0.192552530583097;3.3564288932622;0.837441322382302]
Robot Arm 1 togoal: 16.8151590041609
Step 189:
Mobile Platform 1 Pose: [-4.88352323663406;-3.84243124482488;-5.94374008753796]
Robot Arm 1 end: [-3.182420632079;-6.19879437578773;0.236745761943378]
Robot Arm 1 togoal: 20.9243031400828
Mobile Platform 2 Pose: [5.46108712526073;0.637278851169209;2.79568533441077]
Robot Arm 2 end: [0.103528747039587;3.40247667184853;0.655406745745469]
Robot Arm 1 togoal: 16.8064548838686
Step 190:
Mobile Platform 1 Pose: [-4.93963927937928;-3.71197313306403;-5.95191336786005]
Robot Arm 1 end: [-3.07706408251304;-6.22912649853365;0.18861193006802]
Robot Arm 1 togoal: 20.884669255041
Mobile Platform 2 Pose: [5.55478244920096;0.54462344020327;2.84255561446274]
Robot Arm 2 end: [0.192433231904675;3.5229220195448;0.83861254240705]
Robot Arm 1 togoal: 16.9475888484411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.063287e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-4.91038954192802;-3.62978270274883;-5.95969762708663]
Robot Arm 1 end: [-3.02096016593846;-6.26367517028016;0.183792340170089]
Robot Arm 1 togoal: 20.8767971205559
Mobile Platform 2 Pose: [5.42009751535003;0.569830530210032;2.90913048671198]
Robot Arm 2 end: [0.105529795253843;3.56897155003381;0.656682976328201]
Robot Arm 1 togoal: 16.940654216582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.775155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-4.91323093947267;-3.52846859589289;-5.97140019547804]
Robot Arm 1 end: [-2.95988116528485;-6.26247719005829;0.132189015929906]
Robot Arm 1 togoal: 20.8411967056824
Mobile Platform 2 Pose: [5.49976821513248;0.485165248167823;2.96160792197156]
Robot Arm 2 end: [0.208757624288023;3.67962513820059;0.820014453324108]
Robot Arm 1 togoal: 17.083391258465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.019936e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-4.87020407992922;-3.46334414370587;-5.98011790484141]
Robot Arm 1 end: [-2.92570200070827;-6.37285996843206;0.190161529960777]
Robot Arm 1 togoal: 20.9025224982617
Mobile Platform 2 Pose: [5.36459754408489;0.518648411825239;3.02477865697968]
Robot Arm 2 end: [0.134320974586994;3.7318978356224;0.637012370128947]
Robot Arm 1 togoal: 17.0894971814179

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006960e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-4.91630819166791;-3.35202150200978;-6.00147734946221]
Robot Arm 1 end: [-2.8653954086903;-6.39885164885433;0.138225670607162]
Robot Arm 1 togoal: 20.8890593198715
Mobile Platform 2 Pose: [5.43368720778316;0.442593091043981;3.08078519451646]
Robot Arm 2 end: [0.250336699695878;3.83352887311717;0.771562542767734]
Robot Arm 1 togoal: 17.2336177293916

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-4.88323122346796;-3.29532986184077;-6.01842155185656]
Robot Arm 1 end: [-2.87650616217868;-6.52425652631702;0.196247918504905]
Robot Arm 1 togoal: 20.9907225404363
Mobile Platform 2 Pose: [5.30749520867667;0.474128246768592;3.14933398465484]
Robot Arm 2 end: [0.216608400100176;3.90534466600577;0.586784138088723]
Robot Arm 1 togoal: 17.2803488610056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-4.89523813174018;-3.21175676690431;-6.04707662802714]
Robot Arm 1 end: [-2.89411949031992;-6.58934399318733;0.16611944091602]
Robot Arm 1 togoal: 21.0546990689568
Mobile Platform 2 Pose: [5.38162170640977;0.403699721648899;3.20376650483089]
Robot Arm 2 end: [0.342515555893328;3.97997596296237;0.72805315202483]
Robot Arm 1 togoal: 17.4079326932469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-4.82819762217074;-3.19636867428166;-6.06559473376115]
Robot Arm 1 end: [-2.97279743600693;-6.69617000284962;0.253693588544487]
Robot Arm 1 togoal: 21.1816343000308
Mobile Platform 2 Pose: [5.24813746774849;0.447724363640276;3.27230291185322]
Robot Arm 2 end: [0.340540107439113;4.05506500935958;0.540872346164528]
Robot Arm 1 togoal: 17.4766207359631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001293e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-4.77094537888253;-3.16123081767193;-6.09687726738832]
Robot Arm 1 end: [-3.04501477925864;-6.78214220809869;0.336641469808428]
Robot Arm 1 togoal: 21.2888565407342
Mobile Platform 2 Pose: [5.30760133649132;0.388641486015352;3.33653006369979]
Robot Arm 2 end: [0.478869094748754;4.11064613865602;0.63219726477941]
Robot Arm 1 togoal: 17.5984943179064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000458e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-4.68432818860452;-3.13501632889717;-6.13205165135538]
Robot Arm 1 end: [-3.11933550988918;-6.847948655239;0.435937205204417]
Robot Arm 1 togoal: 21.3810125901291
Mobile Platform 2 Pose: [5.21258787875654;0.413975944696046;3.43051000698972]
Robot Arm 2 end: [0.588887863795291;4.17645878704931;0.462920332790342]
Robot Arm 1 togoal: 17.726140654838

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000412e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-4.51676505792762;-3.09748327572866;-6.17574005747142]
Robot Arm 1 end: [-3.16932842613128;-6.90838562055685;0.508959325333276]
Robot Arm 1 togoal: 21.4557437954489
Mobile Platform 2 Pose: [5.3249871248937;0.340666300795409;3.49195127048917]
Robot Arm 2 end: [0.698081209313373;4.17358018191744;0.628556307267782]
Robot Arm 1 togoal: 17.7802073238476

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001195e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.532886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-4.57078304447758;-2.99424185999287;-6.24941346166608]
Robot Arm 1 end: [-3.20666883454078;-6.92926584535325;0.441814338781768]
Robot Arm 1 togoal: 21.4983902992585
Mobile Platform 2 Pose: [5.12930562049878;0.446037697982244;3.55089189685725]
Robot Arm 2 end: [0.784323637463326;4.23014016677144;0.432940329994054]
Robot Arm 1 togoal: 17.8880518595236

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.565735e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-4.37608176275658;-3.03348910000154;-6.24735480459419]
Robot Arm 1 end: [-3.2881490554394;-6.92791931489473;0.522778270763105]
Robot Arm 1 togoal: 21.5435791889807
Mobile Platform 2 Pose: [4.9334610799239;0.552369209371929;3.62938026299872]
Robot Arm 2 end: [0.810606660412135;4.31916614658677;0.330224628739968]
Robot Arm 1 togoal: 17.9812591740541

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000053e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.503278e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-4.17861546406155;-3.06499066780526;-6.25656849461788]
Robot Arm 1 end: [-3.37323256959547;-6.91479430134274;0.642636367824068]
Robot Arm 1 togoal: 21.5806658443977
Mobile Platform 2 Pose: [4.74268347221037;0.65405268028316;3.67647660013907]
Robot Arm 2 end: [0.801353387547088;4.30994141078183;0.150893187214555]
Robot Arm 1 togoal: 17.9811248497174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994383e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.719989e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-3.9504232809706;-3.03890083580389;-6.23299597629614]
Robot Arm 1 end: [-3.37149730582512;-6.87305291379845;0.72540967763576]
Robot Arm 1 togoal: 21.5436723608713
Mobile Platform 2 Pose: [4.68845641951551;0.668911122086266;3.76508841194704]
Robot Arm 2 end: [0.817051693351924;4.41597321783224;0.183565922920256]
Robot Arm 1 togoal: 18.0725654256511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001345e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.576867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-3.79542063062343;-2.92993853887811;-6.21567246540244]
Robot Arm 1 end: [-3.29659293485516;-6.83459649319285;0.67961663541162]
Robot Arm 1 togoal: 21.4688366878615
Mobile Platform 2 Pose: [4.58289954482898;0.719139814033096;3.81387164481651]
Robot Arm 2 end: [0.895804686311093;4.42203460440131;0.124939942302417]
Robot Arm 1 togoal: 18.1290547348031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-3.81356493558466;-2.82001363939818;-6.14144191223391]
Robot Arm 1 end: [-3.25330908371949;-6.708271885281;0.76794683354624]
Robot Arm 1 togoal: 21.3397197763368
Mobile Platform 2 Pose: [4.44699266688358;0.788456587316884;3.85835515430068]
Robot Arm 2 end: [0.970726081827987;4.52153983560282;0.184406397686544]
Robot Arm 1 togoal: 18.2487926290274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000598e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-3.77654145300506;-2.69347102422781;-6.09911183201225]
Robot Arm 1 end: [-3.1871401903788;-6.59498788594428;0.826635345895485]
Robot Arm 1 togoal: 21.2079332234004
Mobile Platform 2 Pose: [4.37082963014407;0.829590104941013;3.86541497278213]
Robot Arm 2 end: [1.02780131739364;4.52143748186817;0.174934918919846]
Robot Arm 1 togoal: 18.2837753114125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000576e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-3.99955582866374;-2.45213006518655;-6.03701226462894]
Robot Arm 1 end: [-3.00417011018853;-6.46528716261511;0.814880300197729]
Robot Arm 1 togoal: 20.9931590436634
Mobile Platform 2 Pose: [4.2933404795726;0.871621409242241;3.87242247789493]
Robot Arm 2 end: [1.08280550519079;4.52270999945892;0.167612193310145]
Robot Arm 1 togoal: 18.3185518573267

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000736e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-4.22512067407457;-2.27703993840958;-5.98837222601262]
Robot Arm 1 end: [-2.89076643107867;-6.33732197015121;0.772167016497606]
Robot Arm 1 togoal: 20.8240413746754
Mobile Platform 2 Pose: [4.21496270613373;0.914284244510614;3.87937709976089]
Robot Arm 2 end: [1.1361322405342;4.52509577336977;0.161808983503088]
Robot Arm 1 togoal: 18.3531808884334

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001087e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-4.37900318852468;-2.18408613645653;-5.96160260694764]
Robot Arm 1 end: [-2.86217089473651;-6.22203528441877;0.690795463438254]
Robot Arm 1 togoal: 20.7190518235479
Mobile Platform 2 Pose: [4.13599118228842;0.957394719383838;3.88627961321161]
Robot Arm 2 end: [1.18812869170888;4.52833308599761;0.157071965726324]
Robot Arm 1 togoal: 18.3876857043677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000361e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-4.42358577678957;-2.1590891224483;-5.95876432751183]
Robot Arm 1 end: [-2.8576055016726;-6.20465934886306;0.664713306822476]
Robot Arm 1 togoal: 20.7036767447294
Mobile Platform 2 Pose: [4.05667057350555;1.00080217717572;3.89311860496384]
Robot Arm 2 end: [1.23915176241335;4.53214937042702;0.153083109804949]
Robot Arm 1 togoal: 18.4220817032561

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-4.40098409971891;-2.14806216142586;-5.97403364279239]
Robot Arm 1 end: [-2.85985786233813;-6.14154805727575;0.707639227760444]
Robot Arm 1 togoal: 20.6539844502387
Mobile Platform 2 Pose: [3.97723325791054;1.04436876416776;3.89986821398856]
Robot Arm 2 end: [1.29006014670091;4.53752673865688;0.151513974761243]
Robot Arm 1 togoal: 18.4575359614064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997787e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-4.40877191339331;-2.12645149792167;-5.98487653415052]
Robot Arm 1 end: [-2.83754945681899;-6.07214794920671;0.70807575848482]
Robot Arm 1 togoal: 20.5858558622153
Mobile Platform 2 Pose: [3.89818237568993;1.08781296244294;3.90644143044556]
Robot Arm 2 end: [1.34143303778349;4.54396217559215;0.152188932862018]
Robot Arm 1 togoal: 18.4940147846027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-4.43796742348076;-2.09856623150328;-5.99219312461584]
Robot Arm 1 end: [-2.79482492866889;-6.0006846038021;0.683845430070781]
Robot Arm 1 togoal: 20.5043966143749
Mobile Platform 2 Pose: [3.81996198096315;1.13089313555136;3.91274958264829]
Robot Arm 2 end: [1.39390733237308;4.55072857469399;0.154756686393106]
Robot Arm 1 togoal: 18.5313638770895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998383e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-4.4803190458336;-2.06792378567778;-5.9966727726789]
Robot Arm 1 end: [-2.73423724726074;-5.93155803225095;0.652049148817984]
Robot Arm 1 togoal: 20.4139759769201
Mobile Platform 2 Pose: [3.74292669064383;1.17342492575049;3.91870821807699]
Robot Arm 2 end: [1.44818544688266;4.55695944640356;0.158783069849537]
Robot Arm 1 togoal: 18.5693740690213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.749503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-4.53086395546906;-2.0359850342655;-5.99892974454267]
Robot Arm 1 end: [-2.65812423493445;-5.86821958896211;0.622949499923273]
Robot Arm 1 togoal: 20.3183413882936
Mobile Platform 2 Pose: [3.66732975577397;1.21528709081244;3.9242399143293]
Robot Arm 2 end: [1.50500044835548;4.56164679583971;0.163738547524802]
Robot Arm 1 togoal: 18.6077605147375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000517e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-4.58662957087421;-2.00326399484503;-5.99949008532931]
Robot Arm 1 end: [-2.58544980991865;-5.88317091866841;0.576313714245159]
Robot Arm 1 togoal: 20.286968662866
Mobile Platform 2 Pose: [3.59331140689934;1.25642613866511;3.92927647043022]
Robot Arm 2 end: [1.56505918492669;4.56362934680995;0.16898669447073]
Robot Arm 1 togoal: 18.6461211430273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000024e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000265e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-4.65410153100048;-1.9641229858168;-5.99871474331784]
Robot Arm 1 end: [-2.47654480186047;-5.82717352189936;0.549876587522279]
Robot Arm 1 togoal: 20.1768846100533
Mobile Platform 2 Pose: [3.51506987675544;1.29976913466453;3.93376015773811]
Robot Arm 2 end: [1.63458171392757;4.53059122541247;0.135287447181875]
Robot Arm 1 togoal: 18.6660593671275

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001112e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000053e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-4.71840895446068;-1.92839777098803;-5.99694693130522]
Robot Arm 1 end: [-2.3758825037593;-5.82701905736724;0.519613719483828]
Robot Arm 1 togoal: 20.1161570502899
Mobile Platform 2 Pose: [3.43565605298626;1.34428264010373;3.93809351998704]
Robot Arm 2 end: [1.70566877852308;4.51400753406204;0.126528491645384]
Robot Arm 1 togoal: 18.6982257724874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001012e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999800e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-4.78845104496575;-1.89024633891536;-5.99365921995379]
Robot Arm 1 end: [-2.24115745177547;-5.79986314236512;0.490637403477986]
Robot Arm 1 togoal: 20.013576845655
Mobile Platform 2 Pose: [3.35684723044034;1.38885143491749;3.94193608448395]
Robot Arm 2 end: [1.77837944668618;4.49801023077059;0.122844654112514]
Robot Arm 1 togoal: 18.7317207274954

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001526e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999476e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-4.86352185606802;-1.85009467693369;-5.98869422827555]
Robot Arm 1 end: [-2.10104686091799;-5.77383283370833;0.467669771976848]
Robot Arm 1 togoal: 19.9087579212058
Mobile Platform 2 Pose: [3.27171620204898;1.43740495236132;3.94608968926588]
Robot Arm 2 end: [1.84790821145582;4.48559815442133;0.119649475480032]
Robot Arm 1 togoal: 18.7661734557429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000701e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-4.94900576980445;-1.80433531769525;-5.98146457723722]
Robot Arm 1 end: [-1.95180255855606;-5.72848733203732;0.443240989025211]
Robot Arm 1 togoal: 19.7836412886633
Mobile Platform 2 Pose: [3.18529004674983;1.48698848639321;3.94982314390226]
Robot Arm 2 end: [1.91620710214162;4.47168010693144;0.115772618928182]
Robot Arm 1 togoal: 18.7989409491865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005951e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998473e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-5.0016715298927;-1.77531277454559;-5.97281350007203]
Robot Arm 1 end: [-1.90951437422219;-5.69503505550664;0.400853074474985]
Robot Arm 1 togoal: 19.7338639692826
Mobile Platform 2 Pose: [3.0930758852277;1.53974784674754;3.95298052372367]
Robot Arm 2 end: [1.99220052794773;4.45322242693061;0.113356432263986]
Robot Arm 1 togoal: 18.833214607141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.128985e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997887e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-5.05691607996422;-1.74907220339574;-5.96286044111915]
Robot Arm 1 end: [-1.91634118669617;-5.72119199162449;0.326379066241256]
Robot Arm 1 togoal: 19.7631521800204
Mobile Platform 2 Pose: [2.9995270281001;1.59345665688602;3.95538969026136]
Robot Arm 2 end: [2.07504644049101;4.42699966389798;0.112144773084035]
Robot Arm 1 togoal: 18.8661055571237

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.817071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-5.08754397626081;-1.73416292471096;-5.95641769738466]
Robot Arm 1 end: [-1.8843004499336;-5.82877775312069;0.270606452447042]
Robot Arm 1 togoal: 19.8330604387155
Mobile Platform 2 Pose: [2.90424374813567;1.64838135588696;3.95690253229741]
Robot Arm 2 end: [2.16509277039984;4.38992420695249;0.112138700630018]
Robot Arm 1 togoal: 18.8956156543028

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.932296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-5.10888504060339;-1.72151732857336;-5.95008200459829]
Robot Arm 1 end: [-1.81102193594857;-5.95382115577153;0.274238002843718]
Robot Arm 1 togoal: 19.8891695180909
Mobile Platform 2 Pose: [2.80643851475263;1.7050190466257;3.95736257239378]
Robot Arm 2 end: [2.26069115887724;4.34003039464153;0.113541675360665]
Robot Arm 1 togoal: 18.9193540679435

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.042879e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-5.11154492480683;-1.71891172664192;-5.94139617607054]
Robot Arm 1 end: [-1.72876908443882;-6.027818521033;0.292313343632216]
Robot Arm 1 togoal: 19.8988608135941
Mobile Platform 2 Pose: [2.70512334938772;1.76397562055303;3.95661765172303]
Robot Arm 2 end: [2.35593783448593;4.28274603465267;0.11721364621261]
Robot Arm 1 togoal: 18.937704459283

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.398188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992337e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-5.09266000370052;-1.72682762108488;-5.93057224595492]
Robot Arm 1 end: [-1.70254499240115;-6.03037205768723;0.273268963039412]
Robot Arm 1 togoal: 19.8866593561525
Mobile Platform 2 Pose: [2.60002317548483;1.82534773244902;3.95461922533076]
Robot Arm 2 end: [2.44360402470587;4.23761821203376;0.125449851842117]
Robot Arm 1 togoal: 18.9605014092807

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.469995e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989996e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-5.0826436356575;-1.7287193257707;-5.91993658751031]
Robot Arm 1 end: [-1.68210006401477;-6.01617690600607;0.250045983553451]
Robot Arm 1 togoal: 19.8646597932624
Mobile Platform 2 Pose: [2.49366145546633;1.88736507103983;3.95166007138957]
Robot Arm 2 end: [2.50256644728974;4.25353892957716;0.137228695766333]
Robot Arm 1 togoal: 19.0103225329895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.853854e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989302e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-5.09552440011734;-1.7175878647409;-5.90908160032732]
Robot Arm 1 end: [-1.65587629183923;-5.99461440077245;0.226348603673476]
Robot Arm 1 togoal: 19.8333876127622
Mobile Platform 2 Pose: [2.39097664179245;1.94658929789529;3.9491202382916]
Robot Arm 2 end: [2.56305603992035;4.2640562836472;0.150640548391078]
Robot Arm 1 togoal: 19.0570607322286

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.412595e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-5.13887452725388;-1.68974995706585;-5.89712073638587]
Robot Arm 1 end: [-1.62190430217132;-5.96817552943446;0.204223571143756]
Robot Arm 1 togoal: 19.7935643457981
Mobile Platform 2 Pose: [2.28526024043339;2.00675312123374;3.94712464741473]
Robot Arm 2 end: [2.62137528353174;4.27310019807401;0.162029254525194]
Robot Arm 1 togoal: 19.1015000667241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-5.20949552142782;-1.6477622021497;-5.88213916520979]
Robot Arm 1 end: [-1.59007014664742;-5.94723837838777;0.18556972433658]
Robot Arm 1 togoal: 19.7592386473389
Mobile Platform 2 Pose: [2.18804280511579;2.06168552547734;3.94623537623539]
Robot Arm 2 end: [2.68060345046029;4.27439908427582;0.171967210476743]
Robot Arm 1 togoal: 19.1409524985621

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-5.29985096444638;-1.59627697233;-5.86288025515746]
Robot Arm 1 end: [-1.55811757839956;-5.92626550629221;0.171995886499155]
Robot Arm 1 togoal: 19.7244981439646
Mobile Platform 2 Pose: [2.09029042119397;2.11753074579899;3.94656879420329]
Robot Arm 2 end: [2.74537099173522;4.26939302642408;0.180680074525416]
Robot Arm 1 togoal: 19.1795859330213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-5.38873731230451;-1.5472787271537;-5.83991915686792]
Robot Arm 1 end: [-1.52939240380265;-5.90146617776121;0.165100407964379]
Robot Arm 1 togoal: 19.6881177238208
Mobile Platform 2 Pose: [1.98337170429532;2.17923137931554;3.94763548874775]
Robot Arm 2 end: [2.81680974694564;4.26099808209417;0.187970023361791]
Robot Arm 1 togoal: 19.2203966479756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-5.45419157403593;-1.51078526258815;-5.81682142701499]
Robot Arm 1 end: [-1.51050370487031;-5.87672471520286;0.163579547175052]
Robot Arm 1 togoal: 19.6571786761965
Mobile Platform 2 Pose: [1.87159164859661;2.24441280693305;3.94967172249653]
Robot Arm 2 end: [2.89232505065571;4.2497607646654;0.193631708065273]
Robot Arm 1 togoal: 19.2621295199777

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-5.50125993719497;-1.48177970750476;-5.79755184528371]
Robot Arm 1 end: [-1.50079256125781;-5.85561803864355;0.166384704479627]
Robot Arm 1 togoal: 19.6342515906082
Mobile Platform 2 Pose: [1.75557174121068;2.31283257582646;3.95271049362134]
Robot Arm 2 end: [2.97042705764265;4.23777233583027;0.19798637753513]
Robot Arm 1 togoal: 19.3053491082081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-5.53952586229882;-1.45515247293205;-5.78422628005715]
Robot Arm 1 end: [-1.53551802300816;-5.84646838678656;0.131980203092463]
Robot Arm 1 togoal: 19.6496338996118
Mobile Platform 2 Pose: [1.63551004241928;2.38448138303002;3.95673339144992]
Robot Arm 2 end: [3.05013194583418;4.22734856923109;0.201506478575818]
Robot Arm 1 togoal: 19.3510840592243

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-5.56521184056994;-1.43705363939486;-5.77723594577023]
Robot Arm 1 end: [-1.57734354642681;-5.84803032432394;0.103887294510459]
Robot Arm 1 togoal: 19.6774784251627
Mobile Platform 2 Pose: [1.49348397912966;2.46815217573124;3.96082725058822]
Robot Arm 2 end: [3.13855586427521;4.22121264437906;0.204794569141918]
Robot Arm 1 togoal: 19.4061056374417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.783245e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-5.58058563689973;-1.4278065881187;-5.77127706171477]
Robot Arm 1 end: [-1.62169501368054;-5.8613834685224;0.0804316790492215]
Robot Arm 1 togoal: 19.7160472106822
Mobile Platform 2 Pose: [1.3423999847351;2.55829015584063;3.96719783920453]
Robot Arm 2 end: [3.22955544854532;4.22800081475759;0.207359512129341]
Robot Arm 1 togoal: 19.4726165648649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.662584e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-5.58924206770234;-1.42497778862373;-5.76468431415525]
Robot Arm 1 end: [-1.66555722668715;-5.90160591060247;0.062771105130927]
Robot Arm 1 togoal: 19.7755588405246
Mobile Platform 2 Pose: [1.19705095227032;2.64967317174401;3.98010620757061]
Robot Arm 2 end: [3.27406014607318;4.25145462489428;0.145973629644743]
Robot Arm 1 togoal: 19.524224164001
Step 241:
Mobile Platform 1 Pose: [-5.59184917229279;-1.42781498917255;-5.75843854396129]
Robot Arm 1 end: [-1.70602316795513;-5.9578127360639;0.0511192029142118]
Robot Arm 1 togoal: 19.8454860178739
Mobile Platform 2 Pose: [0.985406720726211;2.79536865685231;4.02360563321909]
Robot Arm 2 end: [3.42938507035536;4.37868386778486;0.209286252231188]
Robot Arm 1 togoal: 19.7184057695764
Step 242:
Mobile Platform 1 Pose: [-5.58588366088631;-1.43730289852514;-5.75429585978893]
Robot Arm 1 end: [-1.74420579264456;-6.02227409223719;0.0460913607121231]
Robot Arm 1 togoal: 19.9202156685365
Mobile Platform 2 Pose: [0.846973180055268;2.89758206536283;4.07427624959276]
Robot Arm 2 end: [3.51698363655935;4.46721755320926;0.221642185568514]
Robot Arm 1 togoal: 19.8418134527271
Step 243:
Mobile Platform 1 Pose: [-5.56601878385126;-1.45517910258792;-5.75451203635019]
Robot Arm 1 end: [-1.78529499990154;-6.090223821148;0.0492784741960723]
Robot Arm 1 togoal: 19.9988610950438
Mobile Platform 2 Pose: [0.820574373555623;2.94794508277246;4.15355200269324]
Robot Arm 2 end: [3.59854061004696;4.45861078000856;0.221027555682559]
Robot Arm 1 togoal: 19.8912458324475
Step 244:
Mobile Platform 1 Pose: [-5.52621688506202;-1.48267397797085;-5.76073526557294]
Robot Arm 1 end: [-1.83621750800746;-6.15793504319888;0.0633283233112207]
Robot Arm 1 togoal: 20.0823234829568
Mobile Platform 2 Pose: [0.958610784181845;2.91316365500613;4.24166417325158]
Robot Arm 2 end: [3.57022986566176;4.48750038404224;0.235086724338487]
Robot Arm 1 togoal: 19.8920179704585
Step 245:
Mobile Platform 1 Pose: [-5.46360270130163;-1.51957496612919;-5.7728970899008]
Robot Arm 1 end: [-1.90132181724638;-6.2215480267095;0.0904665253747123]
Robot Arm 1 togoal: 20.1699406891178
Mobile Platform 2 Pose: [1.20923873525515;2.79161525261762;4.29523457824339]
Robot Arm 2 end: [3.47828240375459;4.47645524850349;0.288562970914642]
Robot Arm 1 togoal: 19.8179402095159
Step 246:
Mobile Platform 1 Pose: [-5.37947380327057;-1.56482349494233;-5.7899991326818]
Robot Arm 1 end: [-1.97998394649881;-6.27792694765581;0.130470528298963]
Robot Arm 1 togoal: 20.2589361340436
Mobile Platform 2 Pose: [1.52081565602335;2.63776081305585;4.36180560335221]
Robot Arm 2 end: [3.30622076792568;4.59699151496371;0.356196913241134]
Robot Arm 1 togoal: 19.7859656990409
Step 247:
Mobile Platform 1 Pose: [-5.27462240637047;-1.61863159796504;-5.8118867699447]
Robot Arm 1 end: [-2.07148772039922;-6.32438600107268;0.180729429341668]
Robot Arm 1 togoal: 20.3470567723772
Mobile Platform 2 Pose: [1.77223510506945;2.48954103206493;4.37903135544297]
Robot Arm 2 end: [3.19176185880313;4.58155558382248;0.417454930483416]
Robot Arm 1 togoal: 19.694216305909
Step 248:
Mobile Platform 1 Pose: [-5.14751442315753;-1.68324098562737;-5.83913995106636]
Robot Arm 1 end: [-2.17951757031579;-6.3564231816785;0.238695580324941]
Robot Arm 1 togoal: 20.4332457570622
Mobile Platform 2 Pose: [2.11696068536509;2.29701760972425;4.42584094716723]
Robot Arm 2 end: [3.00873587892474;4.66396681402156;0.512716401537504]
Robot Arm 1 togoal: 19.6284428242609
Step 249:
Mobile Platform 1 Pose: [-4.99994932957512;-1.76084756838294;-5.87074925377157]
Robot Arm 1 end: [-2.30965891064187;-6.36264183197917;0.30107336281359]
Robot Arm 1 togoal: 20.5122395466243
Mobile Platform 2 Pose: [2.51993715116284;2.09013403528816;4.53362714359529]
Robot Arm 2 end: [2.83045859856445;4.72439250130732;0.595781054129589]
Robot Arm 1 togoal: 19.5520953475536
Step 250:
Mobile Platform 1 Pose: [-4.84308269347406;-1.85036490854796;-5.90148368682066]
Robot Arm 1 end: [-2.43936726427902;-6.3335810721104;0.309096223677713]
Robot Arm 1 togoal: 20.5667799943959
Mobile Platform 2 Pose: [2.29108643384506;2.2652543069183;4.62809614364834]
Robot Arm 2 end: [2.98209769319024;4.87853802337684;0.445106086893312]
Robot Arm 1 togoal: 19.7753205268806
Step 251:
Mobile Platform 1 Pose: [-4.67273826488839;-1.95737178306525;-5.92770836932595]
Robot Arm 1 end: [-2.58990880006168;-6.27921895015706;0.31112096464959]
Robot Arm 1 togoal: 20.6151056388563
Mobile Platform 2 Pose: [2.46819159060266;2.23061581356406;4.7974914944213]
Robot Arm 2 end: [2.92830813599068;4.97603461879001;0.380039499385506]
Robot Arm 1 togoal: 19.8172792034195
Step 252:
Mobile Platform 1 Pose: [-4.48258665655696;-2.08846267430646;-5.94456230201999]
Robot Arm 1 end: [-2.7630626696641;-6.1892092184965;0.31026900220472]
Robot Arm 1 togoal: 20.650717502766
Mobile Platform 2 Pose: [2.78553768636089;2.05373421582654;4.83955800718468]
Robot Arm 2 end: [2.75089370312375;5.08246058269676;0.454430649620911]
Robot Arm 1 togoal: 19.7788644172544

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.299656e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-4.26734778981716;-2.25728921122724;-5.93843069417026]
Robot Arm 1 end: [-2.9588632655626;-6.05562445356918;0.305250668512641]
Robot Arm 1 togoal: 20.6686540482589
Mobile Platform 2 Pose: [3.12656231783589;1.86292430349095;4.88687464525827]
Robot Arm 2 end: [2.59394581672362;5.09721683765255;0.55849097765181]
Robot Arm 1 togoal: 19.683999875426

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.554531e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-4.17551425707734;-2.39395623964346;-5.86187263390502]
Robot Arm 1 end: [-3.09103501114312;-5.96865399112204;0.302444701300502]
Robot Arm 1 togoal: 20.6847283029568
Mobile Platform 2 Pose: [3.03587982441078;1.9861262918372;5.06305983920282]
Robot Arm 2 end: [2.71832534699032;5.19611287403569;0.399776169322109]
Robot Arm 1 togoal: 19.8477905317143

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962220e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-4.2201430953008;-2.44391521342396;-5.76907728216845]
Robot Arm 1 end: [-3.11520740299259;-5.99340431637826;0.320595546694818]
Robot Arm 1 togoal: 20.7180746620475
Mobile Platform 2 Pose: [2.76127108643209;2.21645770607002;5.16025777993886]
Robot Arm 2 end: [2.8757692506451;5.33319290469789;0.249073980334569]
Robot Arm 1 togoal: 20.0626527221112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928024e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-4.10533325206552;-2.59963245867715;-5.6937172008944]
Robot Arm 1 end: [-3.26619588288183;-5.88219852856732;0.297233815219838]
Robot Arm 1 togoal: 20.7300213935931
Mobile Platform 2 Pose: [2.97731033728181;2.09337409377298;5.17625077781852]
Robot Arm 2 end: [2.80573158632261;5.24767842130046;0.379098162606656]
Robot Arm 1 togoal: 19.9444829089795

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.909735e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-4.19159763730008;-2.62253875303026;-5.59328436893522]
Robot Arm 1 end: [-3.25573187318343;-5.89674833943995;0.331594703473867]
Robot Arm 1 togoal: 20.7324798376996
Mobile Platform 2 Pose: [3.40579053895717;1.83857059481428;5.21054822121973]
Robot Arm 2 end: [2.58680042081659;5.23493851853305;0.560634286703901]
Robot Arm 1 togoal: 19.7851762387962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.512200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-4.10277322008582;-2.76785903829404;-5.50634169708109]
Robot Arm 1 end: [-3.40452156641218;-5.79274265366159;0.325985917548231]
Robot Arm 1 togoal: 20.7489753387113
Mobile Platform 2 Pose: [2.97519725685953;2.19707468645844;5.29727607406482]
Robot Arm 2 end: [2.91833472447367;5.35609758886878;0.377601841636037]
Robot Arm 1 togoal: 20.0997627986241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.561222e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-4.13478658119077;-2.82604814932257;-5.40962243665511]
Robot Arm 1 end: [-3.42047980460081;-5.78423194966468;0.31310495953026]
Robot Arm 1 togoal: 20.7535603416692
Mobile Platform 2 Pose: [3.53112540382088;1.91765386314492;5.39577033888463]
Robot Arm 2 end: [2.65995725732891;5.55245839466115;0.547150053768168]
Robot Arm 1 togoal: 20.0773968330767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984915e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-4.1346466688537;-2.93922549223158;-5.30010109372539]
Robot Arm 1 end: [-3.48940911709558;-5.71886831585053;0.31158926320942]
Robot Arm 1 togoal: 20.7487265582661
Mobile Platform 2 Pose: [3.22543826741287;2.1244495529046;5.43034176536792]
Robot Arm 2 end: [2.84141181167023;5.58132570645174;0.363985454525821]
Robot Arm 1 togoal: 20.2241996317747

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973823e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-4.04176246534278;-3.09587714834216;-5.21280902199701]
Robot Arm 1 end: [-3.60094905822883;-5.70044971744972;0.306714750659446]
Robot Arm 1 togoal: 20.8077972231918
Mobile Platform 2 Pose: [2.85631481205681;2.37828220019423;5.46424997126131]
Robot Arm 2 end: [3.04869203291574;5.6089055650687;0.230515138474513]
Robot Arm 1 togoal: 20.3855745047065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997239e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-3.89710895269465;-3.3137170256696;-5.13835222920771]
Robot Arm 1 end: [-3.81245253993949;-5.57560583599666;0.258197529279701]
Robot Arm 1 togoal: 20.8561264957439
Mobile Platform 2 Pose: [2.63952003854426;2.51792965236936;5.45902627555913]
Robot Arm 2 end: [3.25169070824975;5.55299200877459;0.280644754302633]
Robot Arm 1 togoal: 20.4704994611184

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973501e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-3.92327396594567;-3.40048841099587;-5.01663935187207]
Robot Arm 1 end: [-3.86100397672407;-5.53812761161073;0.296275978409139]
Robot Arm 1 togoal: 20.8581108887292
Mobile Platform 2 Pose: [2.52733022603685;2.59456873681223;5.45925963521971]
Robot Arm 2 end: [3.34846401776332;5.5901026510921;0.319112029770598]
Robot Arm 1 togoal: 20.5591178555427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-3.92231388011789;-3.55024917123943;-4.9010387763976]
Robot Arm 1 end: [-3.93958371536024;-5.44607350460997;0.270096015852758]
Robot Arm 1 togoal: 20.8436547094051
Mobile Platform 2 Pose: [2.40776495439436;2.68177258182635;5.4639687652292]
Robot Arm 2 end: [3.45842375918327;5.60907215919278;0.366930841374521]
Robot Arm 1 togoal: 20.6422910398901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985378e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-3.74548091019286;-3.78772320949755;-4.82689351128755]
Robot Arm 1 end: [-4.1189021715946;-5.43085766253618;0.276340305918154]
Robot Arm 1 togoal: 20.9524209695821
Mobile Platform 2 Pose: [2.2817901877391;2.77871242267256;5.47275953649951]
Robot Arm 2 end: [3.57583838127354;5.63496430074765;0.427437415881632]
Robot Arm 1 togoal: 20.7352688809549

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-3.61788000313748;-4.03359392989974;-4.78164322008982]
Robot Arm 1 end: [-4.34644091221072;-5.34758689813348;0.232014876759719]
Robot Arm 1 togoal: 21.048284326549
Mobile Platform 2 Pose: [2.13786376812631;2.88650079902413;5.48350124394868]
Robot Arm 2 end: [3.69706148132241;5.66753885289275;0.498704879836445]
Robot Arm 1 togoal: 20.8356990341755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-3.50175964543714;-4.26409832334995;-4.68485621370107]
Robot Arm 1 end: [-4.5179907227548;-5.30290741186095;0.229304530801966]
Robot Arm 1 togoal: 21.1333742470797
Mobile Platform 2 Pose: [1.97843178849609;2.99408083864961;5.49021411926559]
Robot Arm 2 end: [3.80779452623296;5.69877262081325;0.579694778158602]
Robot Arm 1 togoal: 20.9282752992054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-3.53250905339262;-4.48475116130043;-4.56733036503602]
Robot Arm 1 end: [-4.66937892043068;-5.20973737194698;0.270607994310824]
Robot Arm 1 togoal: 21.1681427858626
Mobile Platform 2 Pose: [1.84030296751541;3.07786678973698;5.48983344077694]
Robot Arm 2 end: [3.88700338978732;5.70909533138074;0.62147687649618]
Robot Arm 1 togoal: 20.9865118856872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.103777e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-3.37531545757236;-4.76910101344907;-4.46616268091367]
Robot Arm 1 end: [-5.08995638480071;-4.98032314609932;0.24701340332893]
Robot Arm 1 togoal: 21.3011714960761
Mobile Platform 2 Pose: [1.77418605155229;3.11518470309181;5.48696874979591]
Robot Arm 2 end: [3.91696785163565;5.72222722443916;0.588176400861732]
Robot Arm 1 togoal: 21.0176255306699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.860110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-3.38774910275121;-4.76422579623226;-4.45196579644545]
Robot Arm 1 end: [-5.15015029416951;-5.00542365367087;0.362052302279215]
Robot Arm 1 togoal: 21.354928066564
Mobile Platform 2 Pose: [1.72159452113264;3.14439225103395;5.48415033584774]
Robot Arm 2 end: [3.9384847095809;5.71930492190652;0.529354903349405]
Robot Arm 1 togoal: 21.0323860563847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.999783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-3.37876089518366;-4.77962982634854;-4.45033807477278]
Robot Arm 1 end: [-5.2060010816172;-5.0390208841101;0.423069638149722]
Robot Arm 1 togoal: 21.4149496643423
Mobile Platform 2 Pose: [1.66020540003402;3.17878210954989;5.48169948493613]
Robot Arm 2 end: [3.95037799713188;5.74105446124961;0.464715800204618]
Robot Arm 1 togoal: 21.0596670943684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-3.21480658283864;-5.01748653185813;-4.52730681958954]
Robot Arm 1 end: [-5.48060035067538;-5.0856857408649;0.404396105346267]
Robot Arm 1 togoal: 21.6442534116338
Mobile Platform 2 Pose: [1.59722322487209;3.21416252037646;5.47988977312507]
Robot Arm 2 end: [3.95910652755102;5.74359134283178;0.387034642809322]
Robot Arm 1 togoal: 21.0713834583521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-3.1636385225507;-5.14764483115564;-4.74075193463617]
Robot Arm 1 end: [-5.56734423337871;-5.30413242853118;0.294603993628029]
Robot Arm 1 togoal: 21.8646455044433
Mobile Platform 2 Pose: [1.52515900142154;3.25530530846395;5.47901237710723]
Robot Arm 2 end: [3.96137953623089;5.73808589045753;0.29485960264685]
Robot Arm 1 togoal: 21.0739385574725

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-2.92774479604708;-5.38493508715653;-4.81944967543638]
Robot Arm 1 end: [-5.82715193443434;-5.32129742999845;0.24225017267937]
Robot Arm 1 togoal: 22.0652689631243
Mobile Platform 2 Pose: [1.4278653447584;3.31281039991288;5.48077957243025]
Robot Arm 2 end: [3.95959159566864;5.71680064269346;0.180036161749002]
Robot Arm 1 togoal: 21.0638601394789

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-2.92231659747831;-5.63132003329638;-4.86181072367541]
Robot Arm 1 end: [-5.98826348731022;-5.40615103807942;0.174759350486476]
Robot Arm 1 togoal: 22.2437951580699
Mobile Platform 2 Pose: [1.2538116245141;3.4247031937425;5.49844624547668]
Robot Arm 2 end: [4.01339797596362;5.69093826346727;0.0787257796288872]
Robot Arm 1 togoal: 21.0869870144464

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-2.80768337653253;-5.62087037863918;-4.96330144315705]
Robot Arm 1 end: [-6.08407601383885;-5.49100914320436;0.27026118227896]
Robot Arm 1 togoal: 22.3658939701489
Mobile Platform 2 Pose: [0.945382734317559;3.63112244860736;5.53357173810982]
Robot Arm 2 end: [4.18648981709662;5.74779689665963;0.036856283947102]
Robot Arm 1 togoal: 21.2474065005339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-2.63383778014803;-5.79465367019866;-5.04566369656293]
Robot Arm 1 end: [-6.22645051789168;-5.55169912716975;0.183924798059274]
Robot Arm 1 togoal: 22.5153461498429
Mobile Platform 2 Pose: [0.741588309656774;3.75655050727224;5.54337873372771]
Robot Arm 2 end: [4.43533026153234;5.73784040081428;0.162559625730092]
Robot Arm 1 togoal: 21.3986854851288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-2.42257847580685;-5.99537943812632;-5.14069443317497]
Robot Arm 1 end: [-6.47140583084694;-5.60110200353607;0.172585826182647]
Robot Arm 1 togoal: 22.7270722829261
Mobile Platform 2 Pose: [0.684504790049198;3.78856387449272;5.54028501242879]
Robot Arm 2 end: [4.4694750348585;5.72370425675968;0.163950935822442]
Robot Arm 1 togoal: 21.4112625854017

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-2.28179792307363;-6.13613240223907;-5.20725118745048]
Robot Arm 1 end: [-6.6341955435578;-5.66529868028692;0.176565317671763]
Robot Arm 1 togoal: 22.8889831302958
Mobile Platform 2 Pose: [0.615452932849909;3.82843101023824;5.53775102903078]
Robot Arm 2 end: [4.50310090744383;5.73312176099073;0.163521225561754]
Robot Arm 1 togoal: 21.4409375937879

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-2.14872298889913;-6.24924031948601;-5.25075260129338]
Robot Arm 1 end: [-6.78289434260142;-5.66230128840893;0.164420323780083]
Robot Arm 1 togoal: 22.9959460031926
Mobile Platform 2 Pose: [0.543398340651324;3.87054793749556;5.53581944260969]
Robot Arm 2 end: [4.54049050195442;5.73927699816124;0.163759360685456]
Robot Arm 1 togoal: 21.4707430382668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-2.00869378580733;-6.35816005003697;-5.28114933940808]
Robot Arm 1 end: [-6.94881071486145;-5.62475917715621;0.170416370030268]
Robot Arm 1 togoal: 23.0915430435879
Mobile Platform 2 Pose: [0.472021747217374;3.91286988161759;5.53443965351715]
Robot Arm 2 end: [4.58730253610893;5.71622987279597;0.164297548407297]
Robot Arm 1 togoal: 21.4855746545605

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.610604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-1.88599357515394;-6.44570389782882;-5.29550921867525]
Robot Arm 1 end: [-7.08063872921076;-5.55382749300552;0.165765939247451]
Robot Arm 1 togoal: 23.1410118283409
Mobile Platform 2 Pose: [0.380107427591252;3.96902392689751;5.53370463478258]
Robot Arm 2 end: [4.63363871557434;5.74321303654852;0.165223327259053]
Robot Arm 1 togoal: 21.5367190899277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.577105e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-1.77195961487402;-6.52213343662067;-5.30183992300885]
Robot Arm 1 end: [-7.19392243001786;-5.45842013350673;0.165163162490659]
Robot Arm 1 togoal: 23.1610020310742
Mobile Platform 2 Pose: [0.279189866643466;4.03127291679215;5.53431193940449]
Robot Arm 2 end: [4.69551984375784;5.74210259552502;0.166086762112425]
Robot Arm 1 togoal: 21.5779491029273
Step 284:
Mobile Platform 1 Pose: [-1.6709932387113;-6.58699074011068;-5.30327622167347]
Robot Arm 1 end: [-7.27894217118009;-5.3601044992265;0.164957269561934]
Robot Arm 1 togoal: 23.158875289377
Mobile Platform 2 Pose: [0.203019827824571;4.07659092126779;5.53427526890309]
Robot Arm 2 end: [4.74175789987953;5.73840804348853;0.166345700493732]
Robot Arm 1 togoal: 21.606758248131
Step 285:
Mobile Platform 1 Pose: [-1.57899573962115;-6.64440405334679;-5.30214543403662]
Robot Arm 1 end: [-7.34075460981952;-5.3584950350787;0.16497368318991]
Robot Arm 1 togoal: 23.2039640482482
Mobile Platform 2 Pose: [0.130198584503805;4.11977114466118;5.53418166070407]
Robot Arm 2 end: [4.78588909886299;5.73461174999326;0.166472737547565]
Robot Arm 1 togoal: 21.6341227571545
Step 286:
Mobile Platform 1 Pose: [-1.49827437699805;-6.69399897168286;-5.30137526865432]
Robot Arm 1 end: [-7.39336782186579;-5.35744927626681;0.165109074584356]
Robot Arm 1 togoal: 23.2426102807204
Mobile Platform 2 Pose: [0.0571883352763254;4.16310148195891;5.53410669495547]
Robot Arm 2 end: [4.83009461069895;5.73099680428582;0.166480161118925]
Robot Arm 1 togoal: 21.66173332429
Step 287:
Mobile Platform 1 Pose: [-1.42476551575912;-6.73866339827748;-5.30083811120082]
Robot Arm 1 end: [-7.4401861467709;-5.35675071974116;0.165270913769433]
Robot Arm 1 togoal: 23.2771967138988
Mobile Platform 2 Pose: [-0.0157112617267958;4.20638840610874;5.53401362302627]
Robot Arm 2 end: [4.87415247317732;5.7276014097666;0.166382012369406]
Robot Arm 1 togoal: 21.6894639812106
Step 288:
Mobile Platform 1 Pose: [-1.35647113784934;-6.77981647306424;-5.30047441344095]
Robot Arm 1 end: [-7.4829360444177;-5.35620996210669;0.165444200942195]
Robot Arm 1 togoal: 23.3088774645598
Mobile Platform 2 Pose: [-0.0882715145951869;4.24948402929663;5.5338764788829]
Robot Arm 2 end: [4.917911424102;5.72441977076872;0.166193203513565]
Robot Arm 1 togoal: 21.7172042362847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.394965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-1.2921988712652;-6.81829837632497;-5.30024616283618]
Robot Arm 1 end: [-7.52264936313816;-5.35570995325155;0.165631049004563]
Robot Arm 1 togoal: 23.3383395695837
Mobile Platform 2 Pose: [-0.160318137148062;4.29227548556003;5.53367697984137]
Robot Arm 2 end: [4.96181772286394;5.71801757571784;0.165246563859628]
Robot Arm 1 togoal: 21.7428179742734

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.124936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-1.2311429127175;-6.85466954485907;-5.3001253045942]
Robot Arm 1 end: [-7.56000800927886;-5.35518991819574;0.165835501346871]
Robot Arm 1 togoal: 23.3660482223291
Mobile Platform 2 Pose: [-0.24017096579848;4.34024311739054;5.53361791127998]
Robot Arm 2 end: [5.01116770251551;5.71092876698661;0.163931347729467]
Robot Arm 1 togoal: 21.7717724092768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956290e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-1.17271339636861;-6.88933424868505;-5.30008986190368]
Robot Arm 1 end: [-7.5954939399885;-5.35462506742503;0.166060076057041]
Robot Arm 1 togoal: 23.3923444774324
Mobile Platform 2 Pose: [-0.320218081000458;4.38847358438184;5.5333495205244]
Robot Arm 2 end: [5.06098727152048;5.70367883353706;0.162534871659171]
Robot Arm 1 togoal: 21.8010158938683

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-1.11645680814859;-6.92259902690813;-5.3001222248345]
Robot Arm 1 end: [-7.62946383397015;-5.35401139247514;0.166305513902503]
Robot Arm 1 togoal: 23.4174904305346
Mobile Platform 2 Pose: [-0.397953844719835;4.43533610731481;5.53274843952964]
Robot Arm 2 end: [5.10949180152415;5.6967797424101;0.161196452863479]
Robot Arm 1 togoal: 21.8296748136308

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-1.06201424256756;-6.95470359565188;-5.300208169102]
Robot Arm 1 end: [-7.66219012867194;-5.35335582618976;0.1665712438853]
Robot Arm 1 togoal: 23.4416930387364
Mobile Platform 2 Pose: [-0.472632233463775;4.4803514244679;5.53180561447518]
Robot Arm 2 end: [5.15598777984537;5.69085155951506;0.159936021289395]
Robot Arm 1 togoal: 21.8577053553135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967891e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-1.00909692947754;-6.98583895431091;-5.3003361780528]
Robot Arm 1 end: [-7.69388547450889;-5.35267031252954;0.166855875314298]
Robot Arm 1 togoal: 23.4651182656445
Mobile Platform 2 Pose: [-0.544249522937798;4.52351779023294;5.53056722124439]
Robot Arm 2 end: [5.20030999850436;5.68636771263371;0.158576000139023]
Robot Arm 1 togoal: 21.8853319650853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-0.957470353658762;-7.01615901211454;-5.30049692631859]
Robot Arm 1 end: [-7.72471835086176;-5.35196839648484;0.167157568077891]
Robot Arm 1 togoal: 23.4879003658164
Mobile Platform 2 Pose: [-0.612928478560348;4.56491231297491;5.52907904507033]
Robot Arm 2 end: [5.24237848000184;5.68371473960406;0.157030793203502]
Robot Arm 1 togoal: 21.9127684640254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-0.906943057008038;-7.04578864692676;-5.30068286982847]
Robot Arm 1 end: [-7.75482364053608;-5.35126335205012;0.167474282394114]
Robot Arm 1 togoal: 23.5101484656894
Mobile Platform 2 Pose: [-0.678917150590827;4.60468472403468;5.52738684034792]
Robot Arm 2 end: [5.28213187004854;5.68328308950487;0.155218346117266]
Robot Arm 1 togoal: 21.9402434463519

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975684e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-0.857358250873116;-7.07482962436006;-5.30088791485601]
Robot Arm 1 end: [-7.7843101405753;-5.3505672221584;0.167803937662575]
Robot Arm 1 togoal: 23.531951487314
Mobile Platform 2 Pose: [-0.752276532805101;4.64789106447127;5.52553164602786]
Robot Arm 2 end: [5.32455316464793;5.68502268224781;0.153150672126877]
Robot Arm 1 togoal: 21.9711828999376

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977364e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-0.808587279656602;-7.10336512444522;-5.30110714906068]
Robot Arm 1 end: [-7.81326610117601;-5.34989038969511;0.16814451078557]
Robot Arm 1 togoal: 23.5533819682367
Mobile Platform 2 Pose: [-0.833296160291317;4.69468376147548;5.52354003325695]
Robot Arm 2 end: [5.36993168757084;5.68860687965737;0.150989176412206]
Robot Arm 1 togoal: 22.005547381553

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-0.760524404750501;-7.13146329528958;-5.30133662222252]
Robot Arm 1 end: [-7.84176342601906;-5.34924145378463;0.168494093964385]
Robot Arm 1 togoal: 23.5744990950492
Mobile Platform 2 Pose: [-0.892654145513481;4.73038879450489;5.5214156748347]
Robot Arm 2 end: [5.40364238546593;5.69415519564588;0.148982524401216]
Robot Arm 1 togoal: 22.0331826570219

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980288e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-0.713082586999632;-7.15918008802355;-5.30157316715888]
Robot Arm 1 end: [-7.86986092291241;-5.34862727671743;0.168850926111381]
Robot Arm 1 togoal: 23.5953511480936
Mobile Platform 2 Pose: [-0.950064294395483;4.76485778886111;5.5191505351919]
Robot Arm 2 end: [5.43525028596798;5.70213280783542;0.147399794256559]
Robot Arm 1 togoal: 22.0610675995201

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-0.666190053951011;-7.18656153957013;-5.3018142532884]
Robot Arm 1 end: [-7.89760685590609;-5.3480531214716;0.169213407007029]
Robot Arm 1 togoal: 23.6159774894262
Mobile Platform 2 Pose: [-1.00560734647974;4.79810477306155;5.51674558760352]
Robot Arm 2 end: [5.46470146119764;5.71263128517919;0.146732187848739]
Robot Arm 1 togoal: 22.0891915924389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983213e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-0.619787498105879;-7.21364562013219;-5.30205786681362]
Robot Arm 1 end: [-7.92504096919758;-5.34752283196328;0.169580100199256]
Robot Arm 1 togoal: 23.6364101878992
Mobile Platform 2 Pose: [-1.05281392497226;4.82690798129445;5.51423068842528]
Robot Arm 2 end: [5.48857282596613;5.72620798616906;0.147627040145686]
Robot Arm 1 togoal: 22.1155072086616

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-0.573825788725966;-7.24046373212402;-5.30230241267362]
Robot Arm 1 end: [-7.95219610334862;-5.34703902742644;0.169949728654171]
Robot Arm 1 togoal: 23.6566753513282
Mobile Platform 2 Pose: [-1.10507953784729;4.85787335107234;5.51163472357277]
Robot Arm 2 end: [5.51396491207916;5.74179810882805;0.150814844619928]
Robot Arm 1 togoal: 22.1441803269471

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985364e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-0.528264103945783;-7.26704192749859;-5.30254663434629]
Robot Arm 1 end: [-7.97909949215788;-5.34660329406107;0.170321165894049]
Robot Arm 1 togoal: 23.6767942198073
Mobile Platform 2 Pose: [-1.15603908575021;4.8878551655822;5.50905579772223]
Robot Arm 2 end: [5.53839843434587;5.75834710073192;0.156828402362259]
Robot Arm 1 togoal: 22.1726925157823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-0.483068407484637;-7.29340189651716;-5.30278954831383]
Robot Arm 1 end: [-8.00577380677128;-5.34621636437419;0.170693424520587]
Robot Arm 1 togoal: 23.6967840630449
Mobile Platform 2 Pose: [-1.20628794508386;4.91722024651305;5.50662144889714]
Robot Arm 2 end: [5.57662336296672;5.72694605654403;0.160835631658633]
Robot Arm 1 togoal: 22.1769717221732

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-0.438210207905657;-7.31956177058441;-5.30303039059411]
Robot Arm 1 end: [-8.03223799821983;-5.34587827918665;0.171065643462464]
Robot Arm 1 togoal: 23.7166589162769
Mobile Platform 2 Pose: [-1.27310364735615;4.95749529247366;5.50492862379016]
Robot Arm 2 end: [5.60061000547772;5.76832959169982;0.165594767356548]
Robot Arm 1 togoal: 22.222879516394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-0.393665548712829;-7.34553677410002;-5.3032685731716]
Robot Arm 1 end: [-8.05850797866934;-5.3455885303869;0.171437074816973]
Robot Arm 1 togoal: 23.7364301833413
Mobile Platform 2 Pose: [-1.34241985953473;4.99920403975206;5.50446330788023]
Robot Arm 2 end: [5.64440855209897;5.76426696757218;0.168927165535936]
Robot Arm 1 togoal: 22.2505670246754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-0.349414187671244;-7.37133975312451;-5.30350364860946]
Robot Arm 1 end: [-8.08459717267726;-5.34534618391009;0.171807070995859]
Robot Arm 1 togoal: 23.756107129783
Mobile Platform 2 Pose: [-1.42229970056169;5.04815715639;5.50493006842584]
Robot Arm 2 end: [5.68980494871653;5.7385410763948;0.151961923801942]
Robot Arm 1 togoal: 22.2653524516504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-0.305438929993868;-7.39698160424615;-5.30373528135483]
Robot Arm 1 end: [-8.110516963919;-5.34514998397825;0.172175072529811]
Robot Arm 1 togoal: 23.7756972855929
Mobile Platform 2 Pose: [-1.52563547568328;5.10845486538815;5.50564649603183]
Robot Arm 2 end: [5.76540730588275;5.74108905792797;0.172646907249018]
Robot Arm 1 togoal: 22.3192295636013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-0.261725086847473;-7.42247162239542;-5.30396322453996]
Robot Arm 1 end: [-8.13627705750959;-5.34499844004262;0.172540596796762]
Robot Arm 1 togoal: 23.7952067735026
Mobile Platform 2 Pose: [-1.61382253303193;5.1591910501175;5.50579224838661]
Robot Arm 2 end: [5.8194558578075;5.73225067108227;0.17323165247661]
Robot Arm 1 togoal: 22.351182839695

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-0.218260035662855;-7.44781778296232;-5.30418730128108]
Robot Arm 1 end: [-8.16188577414841;-5.34488989823765;0.172903227806046]
Robot Arm 1 togoal: 23.814640576113
Mobile Platform 2 Pose: [-1.69963172591306;5.20878397224405;5.506109010983]
Robot Arm 2 end: [5.87232397180667;5.72315254288803;0.173451350421249]
Robot Arm 1 togoal: 22.3822269114558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-0.175032863018861;-7.47302697071944;-5.30440738964901]
Robot Arm 1 end: [-8.18735028916141;-5.34482259926567;0.173262607085142]
Robot Arm 1 togoal: 23.8340027528708
Mobile Platform 2 Pose: [-1.78360585598756;5.25765240562373;5.50654911194536]
Robot Arm 2 end: [5.92413395263961;5.71446414535733;0.173465897421272]
Robot Arm 1 togoal: 22.4129041776148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-0.132034074415399;-7.49810516571482;-5.3046234106296]
Robot Arm 1 end: [-8.2126768269897;-5.34479472460396;0.173618425656205]
Robot Arm 1 togoal: 23.8532966160127
Mobile Platform 2 Pose: [-1.865416730642;5.30562176667942;5.50705440013572]
Robot Arm 2 end: [5.97458496228187;5.70656939057713;0.173345358070372]
Robot Arm 1 togoal: 22.4432586709705

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-0.0892553581740755;-7.5230575943824;-5.30483531851148]
Robot Arm 1 end: [-8.23787081964361;-5.3448044328151;0.173970417049826]
Robot Arm 1 togoal: 23.8725248730144
Mobile Platform 2 Pose: [-1.94473893982449;5.35248534898108;5.50757858774075]
Robot Arm 2 end: [6.02346017145605;5.69962070606943;0.173124411991612]
Robot Arm 1 togoal: 22.4732315162157

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-0.0466893931069452;-7.54788885255221;-5.30504309323885]
Robot Arm 1 end: [-8.26293703600406;-5.34484988758333;0.17431835127895]
Robot Arm 1 togoal: 23.8916897417599
Mobile Platform 2 Pose: [-2.02107604746422;5.39794788151151;5.50808762935015]
Robot Arm 2 end: [6.07051202144686;5.69362058968978;0.172821954971589]
Robot Arm 1 togoal: 22.5026369096091

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-0.00432969156033772;-7.57260300576569;-5.30524673435009]
Robot Arm 1 end: [-8.28787968753158;-5.34492927891958;0.174662029682549]
Robot Arm 1 togoal: 23.9107930435398
Mobile Platform 2 Pose: [-2.12564306577783;5.45880990356999;5.50876753586727]
Robot Arm 2 end: [6.13248796550813;5.6886539550725;0.172517861390734]
Robot Arm 1 togoal: 22.5435009472226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [0.0378295289542519;-7.59720367126245;-5.30544625619226]
Robot Arm 1 end: [-8.31270251487449;-5.34504083879281;0.175001280543581]
Robot Arm 1 togoal: 23.9298362780711
Mobile Platform 2 Pose: [-2.20058908088473;5.50400160775753;5.50929225692157]
Robot Arm 2 end: [6.16567687915688;5.68234062569258;0.153169696149521]
Robot Arm 1 togoal: 22.5640394477958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [0.0797934510240527;-7.62169408516126;-5.30564168415899]
Robot Arm 1 end: [-8.33740885900404;-5.34518285226648;0.175335955386341]
Robot Arm 1 togoal: 23.9488206839656
Mobile Platform 2 Pose: [-2.31105514128677;5.57033406682106;5.5110000342002]
Robot Arm 2 end: [6.24718053828358;5.67830348807984;0.17129594503619]
Robot Arm 1 togoal: 22.6186157123707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [0.121566743915304;-7.64607715767476;-5.30583305174651]
Robot Arm 1 end: [-8.36200171980544;-5.34535366505258;0.175665925862698]
Robot Arm 1 togoal: 23.9677472874427
Mobile Platform 2 Pose: [-2.39348971015163;5.61904686480751;5.51109898963908]
Robot Arm 2 end: [6.2959552904281;5.67596658364815;0.170856239587282]
Robot Arm 1 togoal: 22.6520854521844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [0.163153635032982;-7.67035551864448;-5.30602039826178]
Robot Arm 1 end: [-8.38648380448971;-5.34555168824575;0.175991081143433]
Robot Arm 1 togoal: 23.9866169415611
Mobile Platform 2 Pose: [-2.46969200262131;5.6642716175113;5.51112377286549]
Robot Arm 2 end: [6.34138942039981;5.67336209511558;0.170349168732128]
Robot Arm 1 togoal: 22.6830232822207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [0.20455796912918;-7.69453155523691;-5.3062037670485]
Robot Arm 1 end: [-8.41085756773464;-5.34577540086756;0.176311325738813]
Robot Arm 1 togoal: 24.0054303578163
Mobile Platform 2 Pose: [-2.53899929398481;5.70591347452798;5.51108088588417]
Robot Arm 2 end: [6.37402398744449;5.65477197464712;0.151769801131338]
Robot Arm 1 togoal: 22.6948306549553

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.734993e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [0.245783257569692;-7.71860744328136;-5.30638320412323]
Robot Arm 1 end: [-8.43512524509555;-5.34602335073617;0.176626577680999]
Robot Arm 1 togoal: 24.0241881315964
Mobile Platform 2 Pose: [-2.62943443365578;5.76184847325974;5.51192474881398]
Robot Arm 2 end: [6.43297400895249;5.66626834029617;0.158484126338069]
Robot Arm 1 togoal: 22.7449125739154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.354014e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [0.286832719517591;-7.74258517344132;-5.30655875713501]
Robot Arm 1 end: [-8.45928888092923;-5.3462941540788;0.176936767009229]
Robot Arm 1 togoal: 24.0428907627055
Mobile Platform 2 Pose: [-2.71964065980915;5.81797167317939;5.51277475116993]
Robot Arm 2 end: [6.49633130753081;5.66629666533766;0.168293035286255]
Robot Arm 1 togoal: 22.7901648512374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.246429e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [0.327709316530073;-7.76646657317814;-5.30673047457915]
Robot Arm 1 end: [-8.48335035183539;-5.34658649422168;0.177241834506699]
Robot Arm 1 togoal: 24.0615386719272
Mobile Platform 2 Pose: [-2.81042401833821;5.87499068138381;5.51351962061971]
Robot Arm 2 end: [6.54142229995334;5.64644193452213;0.146189959096884]
Robot Arm 1 togoal: 22.8105216854927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.684683e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [0.368415781773565;-7.79025332527887;-5.30689840521021]
Robot Arm 1 end: [-8.50731138642686;-5.34689911962336;0.177541730645413]
Robot Arm 1 togoal: 24.0801322144119
Mobile Platform 2 Pose: [-2.89918144382923;5.92995562584785;5.51418868646103]
Robot Arm 2 end: [6.60832364930961;5.66041238608195;0.166391139711281]
Robot Arm 1 togoal: 22.8674418269018

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.595132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [0.408954644828305;-7.81394698357052;-5.30706259761001]
Robot Arm 1 end: [-8.53117358208464;-5.34723084146037;0.177836414701908]
Robot Arm 1 togoal: 24.0986716905164
Mobile Platform 2 Pose: [-2.97857165210264;5.97890801590051;5.51435431741343]
Robot Arm 2 end: [6.65678984322494;5.65673779470963;0.165128887099018]
Robot Arm 1 togoal: 22.9002286610516

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.978276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [0.449328252866008;-7.83754898632223;-5.30722309987611]
Robot Arm 1 end: [-8.55493841922883;-5.3475805309278;0.17812585401267]
Robot Arm 1 togoal: 24.1171573546023
Mobile Platform 2 Pose: [-3.05697449731266;6.02724078520246;5.51449592261363]
Robot Arm 2 end: [6.70534758743931;5.65164936311254;0.163699177376206]
Robot Arm 1 togoal: 22.9321833091854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000074e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [0.489538788833822;-7.86106066774079;-5.30737995940314]
Robot Arm 1 end: [-8.57860727353594;-5.34794711637958;0.178410023343152]
Robot Arm 1 togoal: 24.1355894221974
Mobile Platform 2 Pose: [-3.13363154989903;6.07451350831763;5.51448199496863]
Robot Arm 2 end: [6.7533040104908;5.64565525207999;0.16220966116837]
Robot Arm 1 togoal: 22.9631451399796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.802694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989394e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [0.52952171129389;-7.88444997995577;-5.30753322273532]
Robot Arm 1 end: [-8.60219193344575;-5.31937023888196;0.184165897279776]
Robot Arm 1 togoal: 24.1352807040418
Mobile Platform 2 Pose: [-3.20756549173629;6.12010770372838;5.51421766727235]
Robot Arm 2 end: [6.79994689475075;5.63908211177732;0.160764716742789]
Robot Arm 1 togoal: 22.9928107240323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.649053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984253e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [0.573691705884828;-7.91060412471888;-5.30721221135234]
Robot Arm 1 end: [-8.62567753100142;-5.28527019751933;0.190471517597092]
Robot Arm 1 togoal: 24.1314246272886
Mobile Platform 2 Pose: [-3.27799879619188;6.16353034308171;5.5136517302432]
Robot Arm 2 end: [6.84476028391261;5.63218474454453;0.159488985039007]
Robot Arm 1 togoal: 23.0209656868236

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [0.637857850873599;-7.94759729487121;-5.30620430641386]
Robot Arm 1 end: [-8.65271936014071;-5.23913265589608;0.196735265126437]
Robot Arm 1 togoal: 24.1227855091446
Mobile Platform 2 Pose: [-3.34171017003563;6.20257420906338;5.51261353387009]
Robot Arm 2 end: [6.8854153808288;5.62556271227869;0.158530888602924]
Robot Arm 1 togoal: 23.0462965514634

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982177e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [0.705989704135063;-7.98801661277206;-5.30383708541925]
Robot Arm 1 end: [-8.68258193615242;-5.2057455962321;0.199355708632577]
Robot Arm 1 togoal: 24.1246826004257
Mobile Platform 2 Pose: [-3.39500222083778;6.23474226911069;5.51093133514325]
Robot Arm 2 end: [6.91909915307625;5.61989087166167;0.157757696616205]
Robot Arm 1 togoal: 23.067191779061

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000372e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [0.777681572937707;-8.03110006622464;-5.30034558443711]
Robot Arm 1 end: [-8.71609882675775;-5.18253351083357;0.198682982357691]
Robot Arm 1 togoal: 24.1360767606439
Mobile Platform 2 Pose: [-3.44381047362793;6.26407817796603;5.50896842657641]
Robot Arm 2 end: [6.94964065493609;5.61597530849446;0.156897225584172]
Robot Arm 1 togoal: 23.0870045598414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000894e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982415e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [0.849381124395335;-8.07477748222776;-5.29596727714678]
Robot Arm 1 end: [-8.75668803022835;-5.17537841542962;0.195416244888339]
Robot Arm 1 togoal: 24.163249537399
Mobile Platform 2 Pose: [-3.48986668039236;6.29172868827561;5.50684003988387]
Robot Arm 2 end: [6.97805284429256;5.61415884050554;0.155703417203339]
Robot Arm 1 togoal: 23.1067148127795

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000963e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [0.919501818139395;-8.11771839314425;-5.29145255999089]
Robot Arm 1 end: [-8.79975304531178;-5.174801062962;0.191834032297202]
Robot Arm 1 togoal: 24.196528346746
Mobile Platform 2 Pose: [-3.5337996955936;6.31810250948272;5.50459225328654]
Robot Arm 2 end: [7.00458247319072;5.61465110185385;0.1541104196455]
Robot Arm 1 togoal: 23.1266413740745

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000720e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983128e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [0.988780350969525;-8.16008488322696;-5.2871252826524]
Robot Arm 1 end: [-8.84035675512035;-5.1706799152246;0.188770986930968]
Robot Arm 1 togoal: 24.2256765827845
Mobile Platform 2 Pose: [-3.57603519786181;6.34346671654558;5.50225318236467]
Robot Arm 2 end: [7.02931979705779;5.6176912377755;0.152089536137028]
Robot Arm 1 togoal: 23.1470068952128

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000580e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [1.05800387512713;-8.2024786438432;-5.28271904694257]
Robot Arm 1 end: [-8.88008815241325;-5.16518207501198;0.185758342424606]
Robot Arm 1 togoal: 24.2533161917146
Mobile Platform 2 Pose: [-3.61692681743906;6.36803676246083;5.499840872461]
Robot Arm 2 end: [7.05236744708735;5.62332787656002;0.149722990302146]
Robot Arm 1 togoal: 23.1679090581413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000498e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983707e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [1.12762814177565;-8.24525931703637;-5.27807966898794]
Robot Arm 1 end: [-8.91995066720743;-5.15964187955862;0.182597881675856]
Robot Arm 1 togoal: 24.28107484022
Mobile Platform 2 Pose: [-3.65676346535153;6.39198412182527;5.49736388740292]
Robot Arm 2 end: [7.07406284847738;5.63106468930818;0.147254755722488]
Robot Arm 1 togoal: 23.1892427964189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000450e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [1.19785931207571;-8.28861070315877;-5.27312043826533]
Robot Arm 1 end: [-8.96060052474977;-5.15491541182094;0.179239435386227]
Robot Arm 1 togoal: 24.3100011273414
Mobile Platform 2 Pose: [-3.695732257075;6.41541970630342;5.49481516713105]
Robot Arm 2 end: [7.09122563909486;5.64947753595817;0.146119206283661]
Robot Arm 1 togoal: 23.2143584488532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001351e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993884e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [1.26408957258228;-8.33027419426759;-5.26780417475138]
Robot Arm 1 end: [-9.01267555049073;-5.19438083067847;0.168027207120239]
Robot Arm 1 togoal: 24.3758400018172
Mobile Platform 2 Pose: [-3.75219235435797;6.44732712710935;5.4922404028177]
Robot Arm 2 end: [7.11328942936291;5.67284095843287;0.145378750898047]
Robot Arm 1 togoal: 23.2463965513465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001828e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [1.32941163358945;-8.37029166687392;-5.2644152450885]
Robot Arm 1 end: [-9.05481461046991;-5.21512740363838;0.161137941873853]
Robot Arm 1 togoal: 24.4220246205235
Mobile Platform 2 Pose: [-3.80862422654876;6.47912882512663;5.48979130625513]
Robot Arm 2 end: [7.1377882408906;5.69047998750642;0.147074747530429]
Robot Arm 1 togoal: 23.2762243206952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002144e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994748e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [1.39376669470781;-8.40926548116542;-5.26178074052144]
Robot Arm 1 end: [-9.09452627533129;-5.22870528753219;0.156919008375579]
Robot Arm 1 togoal: 24.4617071228796
Mobile Platform 2 Pose: [-3.86448796574852;6.51053061709589;5.48742945450927]
Robot Arm 2 end: [7.16411626498301;5.70355136668421;0.150154665804082]
Robot Arm 1 togoal: 23.3042405929528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002447e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [1.45071741451966;-8.44416988920511;-5.25958335852464]
Robot Arm 1 end: [-9.13022984428464;-5.23828198842759;0.154595257711233]
Robot Arm 1 togoal: 24.4956729602137
Mobile Platform 2 Pose: [-3.91945819617428;6.54136814857932;5.48512971859954]
Robot Arm 2 end: [7.19143261449419;5.71298001535007;0.153675254709501]
Robot Arm 1 togoal: 23.3305094942346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002426e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995536e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [1.50609324415527;-8.47796664903138;-5.25770690683731]
Robot Arm 1 end: [-9.16509789543229;-5.24532332968764;0.153495462086346]
Robot Arm 1 togoal: 24.5273506402431
Mobile Platform 2 Pose: [-3.97341720202714;6.57160384748124;5.48288710627183]
Robot Arm 2 end: [7.21939192283217;5.71927841484391;0.157105398408651]
Robot Arm 1 togoal: 23.3551574723812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002130e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [1.55967401578235;-8.51054770661548;-5.25609624615569]
Robot Arm 1 end: [-9.19906451733614;-5.25068607052807;0.153235635368509]
Robot Arm 1 togoal: 24.5572450270748
Mobile Platform 2 Pose: [-4.02627248929484;6.60121217990328;5.48069810786623]
Robot Arm 2 end: [7.24766393435955;5.72288985016863;0.160066531268693]
Robot Arm 1 togoal: 23.3782658967567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001701e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996278e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [1.61125972674568;-8.54180680892165;-5.2547193913862]
Robot Arm 1 end: [-9.23200177333927;-5.25503614707984;0.153552680185493]
Robot Arm 1 togoal: 24.5856855147646
Mobile Platform 2 Pose: [-4.0793806138398;6.63089224466486;5.4785604620447]
Robot Arm 2 end: [7.27691656967345;5.7239371901401;0.162244572047148]
Robot Arm 1 togoal: 23.4004329709499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001248e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [1.66068604546053;-8.57165397504133;-5.25355725947186]
Robot Arm 1 end: [-9.26366368572129;-5.25901809093781;0.154125688323416]
Robot Arm 1 togoal: 24.6128973727204
Mobile Platform 2 Pose: [-4.1278868065484;6.65747257545694;5.47606277695332]
Robot Arm 2 end: [7.30250398334134;5.72399501460549;0.163080639593611]
Robot Arm 1 togoal: 23.4193212604945

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000816e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996779e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [1.70783445080447;-8.6000231481125;-5.25259862513371]
Robot Arm 1 end: [-9.29382470157743;-5.26293307331328;0.154740963453997]
Robot Arm 1 togoal: 24.6389013831904
Mobile Platform 2 Pose: [-4.17354659027718;6.68239671080436;5.47346139379114]
Robot Arm 2 end: [7.32381294943598;5.72635472347145;0.163110186759621]
Robot Arm 1 togoal: 23.436650629738

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997108e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [1.75269097464621;-8.62691167532247;-5.25183028251472]
Robot Arm 1 end: [-9.32383228598063;-5.27153479899925;0.155613764235877]
Robot Arm 1 togoal: 24.6676822767476
Mobile Platform 2 Pose: [-4.215431613623;6.70550843818357;5.47095138254648]
Robot Arm 2 end: [7.33903153077473;5.73363156149061;0.163243676560481]
Robot Arm 1 togoal: 23.4527755025237

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [1.79349452238838;-8.65119129934028;-5.25138271671467]
Robot Arm 1 end: [-9.35010444532328;-5.27738991358725;0.156336924960348]
Robot Arm 1 togoal: 24.6918506784136
Mobile Platform 2 Pose: [-4.25819578819174;6.72930080624734;5.4687243371538]
Robot Arm 2 end: [7.35695004428649;5.7347056617251;0.160591849126468]
Robot Arm 1 togoal: 23.4668998258812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998575e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [1.83182533137625;-8.67388977139524;-5.25111535830488]
Robot Arm 1 end: [-9.37418935467688;-5.28171994003022;0.156964538732177]
Robot Arm 1 togoal: 24.7133722955
Mobile Platform 2 Pose: [-4.30104935159178;6.75334392399237;5.46670544913378]
Robot Arm 2 end: [7.37631872588806;5.73051598982681;0.154838072118061]
Robot Arm 1 togoal: 23.4787560186446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999501e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [1.87227481042023;-8.69742461616886;-5.25096088905545]
Robot Arm 1 end: [-9.39679930438025;-5.28804988342973;0.155368677107038]
Robot Arm 1 togoal: 24.7351014027173
Mobile Platform 2 Pose: [-4.34310457059183;6.77718270332784;5.46483126668905]
Robot Arm 2 end: [7.39561446918096;5.72191207039858;0.145889605376975]
Robot Arm 1 togoal: 23.4878021642537

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000028e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [1.91144763605275;-8.72023849420084;-5.25085065304676]
Robot Arm 1 end: [-9.41104615547623;-5.27406054227344;0.14752841096013]
Robot Arm 1 togoal: 24.7380690941101
Mobile Platform 2 Pose: [-4.38311455125263;6.800228773396;5.46307763439049]
Robot Arm 2 end: [7.41330411719023;5.71342797518047;0.136753107890018]
Robot Arm 1 togoal: 23.4957680969841

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000126e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [1.94966956655021;-8.74312987720274;-5.25038320384645]
Robot Arm 1 end: [-9.4296467169023;-5.27925314353669;0.141431645204101]
Robot Arm 1 togoal: 24.7562104736057
Mobile Platform 2 Pose: [-4.42185281087843;6.82293985573618;5.46150871208195]
Robot Arm 2 end: [7.42946544588294;5.70308344189251;0.12577458737281]
Robot Arm 1 togoal: 23.5014839800449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998411e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000226e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [1.98034206556792;-8.76215542407039;-5.24994347880247]
Robot Arm 1 end: [-9.44342711953715;-5.27517924800016;0.135692185430868]
Robot Arm 1 togoal: 24.7648346845359
Mobile Platform 2 Pose: [-4.45969176712807;6.84562010441419;5.46019897162022]
Robot Arm 2 end: [7.44428283419857;5.69106221951293;0.113328454432539]
Robot Arm 1 togoal: 23.5051904131054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997629e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000317e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [2.00648378217004;-8.77899789928258;-5.24941168170298]
Robot Arm 1 end: [-9.45838914711982;-5.29775870653757;0.132957789268667]
Robot Arm 1 togoal: 24.7906633800503
Mobile Platform 2 Pose: [-4.49819100818599;6.8692044454971;5.4592512167423]
Robot Arm 2 end: [7.46187888346115;5.67683009373465;0.10175864181316]
Robot Arm 1 togoal: 23.5094547355161

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996628e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000067e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [2.02753690965273;-8.79294340877641;-5.24916406265276]
Robot Arm 1 end: [-9.47061914421074;-5.34440859649269;0.133279288203793]
Robot Arm 1 togoal: 24.8290485884774
Mobile Platform 2 Pose: [-4.55481393376098;6.90245384646505;5.45864319588604]
Robot Arm 2 end: [7.50041236348117;5.67802974145199;0.108370446525508]
Robot Arm 1 togoal: 23.5384912796272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [2.04401826455271;-8.80409674406991;-5.24938808204582]
Robot Arm 1 end: [-9.47580525683376;-5.40101961341427;0.135415846659411]
Robot Arm 1 togoal: 24.8680163861309
Mobile Platform 2 Pose: [-4.60553899303002;6.9325619586783;5.45804420948256]
Robot Arm 2 end: [7.54463919629383;5.66213559985298;0.110874028827164]
Robot Arm 1 togoal: 23.5606729812815

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996735e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001425e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [2.05820028610614;-8.81372913270196;-5.25007481468068]
Robot Arm 1 end: [-9.4746883161356;-5.45805227822491;0.138388228731746]
Robot Arm 1 togoal: 24.9023388571638
Mobile Platform 2 Pose: [-4.65750075532445;6.96380603185476;5.45741781077493]
Robot Arm 2 end: [7.57854512136757;5.65984262248541;0.114656968405618]
Robot Arm 1 togoal: 23.5841836888089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994641e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003023e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [2.05161493888686;-8.81278793833001;-5.25118855011482]
Robot Arm 1 end: [-9.45348349356987;-5.53776400382962;0.144807938294638]
Robot Arm 1 togoal: 24.9349961873669
Mobile Platform 2 Pose: [-4.70220126962971;6.99042665008447;5.45636374852496]
Robot Arm 2 end: [7.60738943808608;5.65407582501915;0.11637028133257]
Robot Arm 1 togoal: 23.6017634384899

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995983e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004737e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [2.0446643384638;-8.81171755735764;-5.25294616323436]
Robot Arm 1 end: [-9.4270273392854;-5.60577891056363;0.151291188466049]
Robot Arm 1 togoal: 24.9564786954235
Mobile Platform 2 Pose: [-4.74925384043508;7.01850106234853;5.45557822836344]
Robot Arm 2 end: [7.64207110258026;5.64306878551765;0.118693415197202]
Robot Arm 1 togoal: 23.6202188299498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006429e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [2.04053838343581;-8.81251646180822;-5.25554389914911]
Robot Arm 1 end: [-9.40383964583992;-5.66576938277005;0.163662595959415]
Robot Arm 1 togoal: 24.9753416029493
Mobile Platform 2 Pose: [-4.79610504808327;7.04645193450143;5.4547247307587]
Robot Arm 2 end: [7.67796564529591;5.63032047034915;0.12120260926041]
Robot Arm 1 togoal: 23.6384606118334

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007627e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [2.0416878879876;-8.81684998451425;-5.25937876189285]
Robot Arm 1 end: [-9.3880274092955;-5.72125259669239;0.184788005801747]
Robot Arm 1 togoal: 24.9967768427459
Mobile Platform 2 Pose: [-4.84086953346252;7.07308327130212;5.45357992788697]
Robot Arm 2 end: [7.71116776103275;5.61842549659255;0.123023652185694]
Robot Arm 1 togoal: 23.6553380090225

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006863e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [2.05133108740399;-8.82701507520672;-5.26523676302037]
Robot Arm 1 end: [-9.38213687569498;-5.77241869614856;0.213223367267394]
Robot Arm 1 togoal: 25.0229235125697
Mobile Platform 2 Pose: [-4.88780311673409;7.10126515186241;5.45252987946637]
Robot Arm 2 end: [7.74492753612094;5.60682331086595;0.124900856299117]
Robot Arm 1 togoal: 23.6728637882664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997222e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006056e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [2.05389616739754;-8.83626410883421;-5.27454562235001]
Robot Arm 1 end: [-9.37032481494028;-5.82664261300934;0.241226905726153]
Robot Arm 1 togoal: 25.0465550327593
Mobile Platform 2 Pose: [-4.92840419847449;7.12530301115595;5.45090327201222]
Robot Arm 2 end: [7.77293391788795;5.59709425778345;0.125945982092817]
Robot Arm 1 togoal: 23.6873954889235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999395e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004381e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [2.07693625739067;-8.86065171051303;-5.29127258985892]
Robot Arm 1 end: [-9.39884479619017;-5.85535126899399;0.266287209948236]
Robot Arm 1 togoal: 25.0854867896835
Mobile Platform 2 Pose: [-4.96727859010374;7.14828169158114;5.44911001806884]
Robot Arm 2 end: [7.79955149479964;5.58828780498434;0.126873823036205]
Robot Arm 1 togoal: 23.7015260669929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999266e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002805e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [2.1135460646105;-8.89531478731338;-5.3146636756577]
Robot Arm 1 end: [-9.43993251218352;-5.88590333984337;0.29112666164212]
Robot Arm 1 togoal: 25.1353434316999
Mobile Platform 2 Pose: [-5.0041480145936;7.17002589327188;5.44714224471466]
Robot Arm 2 end: [7.82443372230299;5.58045792363178;0.127586519817859]
Robot Arm 1 togoal: 23.7150312619989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998980e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001522e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [2.1813442473535;-8.94730079259225;-5.34383108359124]
Robot Arm 1 end: [-9.49918629961919;-5.93041062727066;0.340498867942436]
Robot Arm 1 togoal: 25.2069401559075
Mobile Platform 2 Pose: [-5.0412573067599;7.19202663607021;5.44517735480822]
Robot Arm 2 end: [7.85027905184029;5.56889120086739;0.128328680703863]
Robot Arm 1 togoal: 23.7268280403556

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001144e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [2.31136622212722;-9.03156072777373;-5.38190419364561]
Robot Arm 1 end: [-9.51365501983606;-6.0632357725279;0.444019867679771]
Robot Arm 1 togoal: 25.2976315476846
Mobile Platform 2 Pose: [-5.07419915405922;7.21259137131508;5.44338507383608]
Robot Arm 2 end: [7.86916055142164;5.56598357550503;0.125888838571826]
Robot Arm 1 togoal: 23.7392729172171

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001381e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [2.35373991891789;-9.06228658916755;-5.53321201423128]
Robot Arm 1 end: [-9.53776428250651;-6.23513653930944;0.575125568736843]
Robot Arm 1 togoal: 25.4204360711717
Mobile Platform 2 Pose: [-5.09990960751782;7.22849460031259;5.44119357684038]
Robot Arm 2 end: [7.88214158232759;5.56066142227851;0.12195531287975]
Robot Arm 1 togoal: 23.7457890478661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001638e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [2.32384996809822;-9.01844920343932;-5.67044333048305]
Robot Arm 1 end: [-9.4881513253906;-6.33409177959799;0.538123095916953]
Robot Arm 1 togoal: 25.4471342856958
Mobile Platform 2 Pose: [-5.12739454458921;7.24560952403362;5.43915977257377]
Robot Arm 2 end: [7.89588958761848;5.55623932207258;0.117629126755876]
Robot Arm 1 togoal: 23.7535028108635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001964e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [2.35584517699221;-9.03430923350577;-5.68972355228035]
Robot Arm 1 end: [-9.4396495241809;-6.41951728134496;0.584107591077161]
Robot Arm 1 togoal: 25.4632733316294
Mobile Platform 2 Pose: [-5.15711063277219;7.26419315384005;5.43729903151556]
Robot Arm 2 end: [7.91093295751799;5.55264558545251;0.113130160463709]
Robot Arm 1 togoal: 23.7627523995112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002377e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [2.3478805783068;-9.01721403499624;-5.71985460344843]
Robot Arm 1 end: [-9.40713985739954;-6.48534717757488;0.618580462288663]
Robot Arm 1 togoal: 25.4797588299396
Mobile Platform 2 Pose: [-5.18868340450981;7.28405579719804;5.43561058095196]
Robot Arm 2 end: [7.9270826302324;5.54958804607812;0.10849465583975]
Robot Arm 1 togoal: 23.7732023795953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002950e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [1.95364942062249;-8.76746839097305;-5.70858013957729]
Robot Arm 1 end: [-9.24931198614747;-6.40584939721632;0.445218185543496]
Robot Arm 1 togoal: 25.3149207562753
Mobile Platform 2 Pose: [-5.22182523789828;7.30506102641961;5.43409886716681]
Robot Arm 2 end: [7.94404090314213;5.54694155404521;0.103647018947435]
Robot Arm 1 togoal: 23.7845513956315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.351027e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003926e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [1.65872529702377;-8.58212103833076;-5.77854246545188]
Robot Arm 1 end: [-8.94337100754246;-6.30406414248701;0.215410513177056]
Robot Arm 1 togoal: 25.0275523493576
Mobile Platform 2 Pose: [-5.25849160167867;7.32821115136642;5.43277736227321]
Robot Arm 2 end: [7.96661826686058;5.54359475982831;0.101309255631579]
Robot Arm 1 togoal: 23.7995426807848

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.045174e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [1.67210537324453;-8.59143513534848;-5.78627413120487]
Robot Arm 1 end: [-8.93833357094357;-6.37793589085721;0.237849098077318]
Robot Arm 1 togoal: 25.0707745936671
Mobile Platform 2 Pose: [-5.29868795615033;7.35323675728074;5.43149849590611]
Robot Arm 2 end: [7.99083469686735;5.54124633563228;0.0989854439360647]
Robot Arm 1 togoal: 23.8164354207859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.460058e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [1.68008238917293;-8.5993287280156;-5.79724523067033]
Robot Arm 1 end: [-8.91892534550188;-6.45395098248489;0.265846623937997]
Robot Arm 1 togoal: 25.1044547672346
Mobile Platform 2 Pose: [-5.3386390905568;7.3782361731517;5.43025684416675]
Robot Arm 2 end: [8.01483133165212;5.53984402794345;0.0967132470525445]
Robot Arm 1 togoal: 23.8337888946618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.851388e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [1.67143013874472;-8.60076216154086;-5.81362530937362]
Robot Arm 1 end: [-8.88086887139369;-6.53231189107001;0.302815585349869]
Robot Arm 1 togoal: 25.1254555992618
Mobile Platform 2 Pose: [-5.37831220643194;7.40317060501012;5.42903995875663]
Robot Arm 2 end: [8.03837946897883;5.53949669361259;0.0943429545356855]
Robot Arm 1 togoal: 23.8515081637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [1.62470701003633;-8.58615880056821;-5.83830199380638]
Robot Arm 1 end: [-8.80081845497115;-6.61801797796482;0.362983235616969]
Robot Arm 1 togoal: 25.1191221765279
Mobile Platform 2 Pose: [-5.41771982807052;7.42802158051179;5.42783241475526]
Robot Arm 2 end: [8.06613921132388;5.53946600769183;0.0952457869208749]
Robot Arm 1 togoal: 23.8724352524676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [1.51513840419958;-8.54296597703639;-5.86878010411879]
Robot Arm 1 end: [-8.67577163382431;-6.6992154622642;0.458336491591397]
Robot Arm 1 togoal: 25.0754361259377
Mobile Platform 2 Pose: [-5.45523459364618;7.45165070211203;5.42643083898013]
Robot Arm 2 end: [8.09242371688245;5.54215507659528;0.0975764773972651]
Robot Arm 1 togoal: 23.8939430588768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [1.37946679989823;-8.48617585725888;-5.89594765658326]
Robot Arm 1 end: [-8.58598066654289;-6.74449873966294;0.590766881623444]
Robot Arm 1 togoal: 25.0336223714069
Mobile Platform 2 Pose: [-5.49122982299632;7.4742841697495;5.42485682532396]
Robot Arm 2 end: [8.11706415196064;5.54719839172063;0.10078136811219]
Robot Arm 1 togoal: 23.9156938490218

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [1.20714561054499;-8.41004445133465;-5.91738597894106]
Robot Arm 1 end: [-8.48890004950992;-6.75239126488911;0.745405952612058]
Robot Arm 1 togoal: 24.9616422588548
Mobile Platform 2 Pose: [-5.52600932125918;7.49609730724979;5.42312239145984]
Robot Arm 2 end: [8.13951827305148;5.55423654561916;0.103976614708084]
Robot Arm 1 togoal: 23.9370915729106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [1.02538045307134;-8.32974410675208;-5.9363925505818]
Robot Arm 1 end: [-8.39122750550269;-6.72777385441531;0.889693670768126]
Robot Arm 1 togoal: 24.8687397140456
Mobile Platform 2 Pose: [-5.55982064215049;7.5172240845397;5.42123591201548]
Robot Arm 2 end: [8.15918933393705;5.56285749506232;0.106235710366217]
Robot Arm 1 togoal: 23.9574674928844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [0.860068168505245;-8.25600391441355;-5.95229426478318]
Robot Arm 1 end: [-8.28601267077399;-6.67347157462322;1.02872297948334]
Robot Arm 1 togoal: 24.7512676525952
Mobile Platform 2 Pose: [-5.59287347306778;7.53777318005569;5.41920809871539]
Robot Arm 2 end: [8.17548947803816;5.5725656551459;0.106618807317211]
Robot Arm 1 togoal: 23.9761067108136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [0.689005552914673;-8.17788140140284;-5.96775463692585]
Robot Arm 1 end: [-8.17220109666894;-6.5937570846321;1.15007280885312]
Robot Arm 1 togoal: 24.6113643234723
Mobile Platform 2 Pose: [-5.62539609398987;7.55786828226116;5.41705769692038]
Robot Arm 2 end: [8.18792375127154;5.58276020398183;0.104222495854164]
Robot Arm 1 togoal: 23.9922955227948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [0.526225601806011;-8.10087785786806;-5.98232418917855]
Robot Arm 1 end: [-8.04726972398051;-6.48957190376075;1.25353961250414]
Robot Arm 1 togoal: 24.4475246419403
Mobile Platform 2 Pose: [-5.65771406869032;7.57770042260126;5.41481520117917]
Robot Arm 2 end: [8.19618139349385;5.59272348736736;0.0982402247743129]
Robot Arm 1 togoal: 24.0053804059929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [0.354637472418645;-8.0157605397271;-5.99646798280303]
Robot Arm 1 end: [-7.90908013432205;-6.35981823914233;1.33445849165707]
Robot Arm 1 togoal: 24.2572381323529
Mobile Platform 2 Pose: [-5.69032976220631;7.59757825187779;5.41252421774526]
Robot Arm 2 end: [8.20021372223961;5.60161524497858;0.0880101131840305]
Robot Arm 1 togoal: 24.0148212111451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001849e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [0.178303382488172;-7.92267286411883;-6.00960993378475]
Robot Arm 1 end: [-7.76057683832035;-6.15997854565578;1.39212559482593]
Robot Arm 1 togoal: 24.0124885182325
Mobile Platform 2 Pose: [-5.72398581419472;7.61796710737011;5.41024201277924]
Robot Arm 2 end: [8.20691112844345;5.60895765065415;0.0800884276928565]
Robot Arm 1 togoal: 24.0251409556356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024975e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [-0.179408656318961;-7.70246224955038;-6.02577960881848]
Robot Arm 1 end: [-7.58461481582817;-6.07076125159678;1.40040879741096]
Robot Arm 1 togoal: 23.8223128109974
Mobile Platform 2 Pose: [-5.75879043883066;7.63897257910693;5.40800113788245]
Robot Arm 2 end: [8.2218263698173;5.61524562098739;0.0798078986933377]
Robot Arm 1 togoal: 24.0405467227433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005334e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [-0.593283546211546;-7.44361918948812;-6.05244850524331]
Robot Arm 1 end: [-7.3874531391614;-5.89299042385381;1.37714681852948]
Robot Arm 1 togoal: 23.5569812373543
Mobile Platform 2 Pose: [-5.79391022783621;7.66012613302095;5.40577934662542]
Robot Arm 2 end: [8.2370405429191;5.6211563413578;0.0798075727228293]
Robot Arm 1 togoal: 24.055918374023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.142510e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [-0.316078529778085;-7.55749801840593;-6.01902692100215]
Robot Arm 1 end: [-7.5141547197909;-5.83188083787154;1.38803943273054]
Robot Arm 1 togoal: 23.6095689271487
Mobile Platform 2 Pose: [-5.82937704002434;7.68145271431094;5.4035898556483]
Robot Arm 2 end: [8.25270183348417;5.62659374172727;0.0800624156932272]
Robot Arm 1 togoal: 24.071307760754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.804672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [-0.231402855839368;-7.58635217595487;-6.0039855268475]
Robot Arm 1 end: [-7.44304919644687;-5.78349447401141;1.38450539180986]
Robot Arm 1 togoal: 23.5243978426416
Mobile Platform 2 Pose: [-5.86519827123867;7.70296236071991;5.40144266799228]
Robot Arm 2 end: [8.26891302304998;5.63149825733467;0.0805369251970955]
Robot Arm 1 togoal: 24.086756831975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.865310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [-0.114269085752272;-7.61492930904291;-5.97884888970399]
Robot Arm 1 end: [-7.3763643162693;-5.7206606145426;1.35837481027777]
Robot Arm 1 togoal: 23.4329608449654
Mobile Platform 2 Pose: [-5.9013606767508;7.72465226657315;5.39934457975155]
Robot Arm 2 end: [8.28573306407744;5.63584282073264;0.0811908832256017]
Robot Arm 1 togoal: 24.1022958387391

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952292e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [0.0518781049289073;-7.62812891627899;-5.9225969421936]
Robot Arm 1 end: [-7.42922240734617;-5.82317968955241;1.43239733763959]
Robot Arm 1 togoal: 23.540574751785
Mobile Platform 2 Pose: [-5.93783526134346;7.74650958707313;5.39729945530485]
Robot Arm 2 end: [8.30318188065628;5.63962705305522;0.0819838051203107]
Robot Arm 1 togoal: 24.1179428833424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.917828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [-0.218309907715505;-7.36550618013767;-5.85226905403538]
Robot Arm 1 end: [-7.29791449176489;-5.54652510662488;1.30733786482449]
Robot Arm 1 togoal: 23.2585025348438
Mobile Platform 2 Pose: [-5.97458245104039;7.76851449499616;5.39530868396375]
Robot Arm 2 end: [8.32124746565391;5.64287063876202;0.0828779648665605]
Robot Arm 1 togoal: 24.1337047613231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.897810e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [-0.191438367724212;-7.37457393749738;-5.84571844365641]
Robot Arm 1 end: [-7.36748997175161;-5.59308123631538;1.37544331213792]
Robot Arm 1 togoal: 23.340845681136
Mobile Platform 2 Pose: [-6.01155686991002;7.79064306079597;5.39337172107256]
Robot Arm 2 end: [8.33989402654127;5.64560683522981;0.0838401264012124]
Robot Arm 1 togoal: 24.1495787675404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817784e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [-0.110899998456998;-7.41396901949764;-5.83715305961201]
Robot Arm 1 end: [-7.45482419827902;-5.6696257180863;1.43356438288064]
Robot Arm 1 togoal: 23.4566771501182
Mobile Platform 2 Pose: [-6.04871128084458;7.81286966883115;5.39148662917653]
Robot Arm 2 end: [8.35906999064579;5.64787680109083;0.084842248361526]
Robot Arm 1 togoal: 24.1655549986161

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000586e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [0.0217330704404574;-7.48450389358548;-5.82865753988468]
Robot Arm 1 end: [-7.55474731404144;-5.78146641029501;1.47558294265874]
Robot Arm 1 togoal: 23.6056327605979
Mobile Platform 2 Pose: [-6.08599950886767;7.8351688426555;5.38965056096751]
Robot Arm 2 end: [8.37871502021767;5.64972509921514;0.0858614811145655]
Robot Arm 1 togoal: 24.1816187225704

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996874e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [0.205338735696277;-7.58911913516756;-5.82753653291228]
Robot Arm 1 end: [-7.58206682983116;-5.86421843499111;1.49745888422556]
Robot Arm 1 togoal: 23.6812806821315
Mobile Platform 2 Pose: [-6.12337838138529;7.85751648595548;5.38786015346136]
Robot Arm 2 end: [8.3987655824386;5.65119643777287;0.0868797343769205]
Robot Arm 1 togoal: 24.1977524953776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.707461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000988e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.3422884462757;9.99051085033267;9.81851213729428]
Robot Arm 2 end: [9.7684100531604;9.8291469599229;1.48915541841285]
Robot Arm 1 togoal: 27.9997521286437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.14402888378555;9.85638936449442;9.81439386821025]
Robot Arm 2 end: [9.54310170111484;9.88344276701623;1.46008800771489]
Robot Arm 1 togoal: 27.8798821848323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-1.98950876968861;9.74126318470668;9.8270202446641]
Robot Arm 2 end: [9.24543233140894;9.99519959542145;1.34361382595982]
Robot Arm 1 togoal: 27.7529482819091

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.88331754382565;9.64899081939311;9.85035946255923]
Robot Arm 2 end: [9.00272346183014;9.99796620870642;1.21013964285643]
Robot Arm 1 togoal: 27.5883813518118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.918668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.74229482327599;9.52888310191578;9.8689571832791]
Robot Arm 2 end: [8.821460464141;10.0489703425046;1.13109617513765]
Robot Arm 1 togoal: 27.5020110317499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805458e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.58815495362934;9.45285138827291;9.86163605082996]
Robot Arm 2 end: [8.6838109470445;9.97489003345506;1.04408662524918]
Robot Arm 1 togoal: 27.355227466072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883003e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.50736730731746;9.40644608986451;9.85674527453719]
Robot Arm 2 end: [8.64615931754815;9.81194603610059;1.06804090053895]
Robot Arm 1 togoal: 27.2102487121771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.920827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.44446677554851;9.36873435159765;9.84903983940663]
Robot Arm 2 end: [8.65948819594744;9.67962398490933;1.10482602079917]
Robot Arm 1 togoal: 27.1226344720251

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955531e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.38356733390876;9.33342031126155;9.83831911640604]
Robot Arm 2 end: [8.66135518191912;9.58237521768612;1.08920775301556]
Robot Arm 1 togoal: 27.053688120782

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.32649423544703;9.30096288360831;9.82456406089981]
Robot Arm 2 end: [8.63278037285194;9.47990341232923;1.01415202625]
Robot Arm 1 togoal: 26.9611579173652

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.26991744482906;9.27118286541986;9.8121871578064]
Robot Arm 2 end: [8.64096608022356;9.33554145228424;0.9339788854742]
Robot Arm 1 togoal: 26.8643513498083

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.18026086067703;9.22711451634269;9.79773355814331]
Robot Arm 2 end: [8.69078665513557;9.18387083673268;0.855169581510046]
Robot Arm 1 togoal: 26.7919714880115

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.02960702675595;9.15182040521316;9.77129760206304]
Robot Arm 2 end: [8.75898045930928;9.04379221848256;0.771168901118398]
Robot Arm 1 togoal: 26.7418701030862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.792312775447892;9.02364425098813;9.70480111867668]
Robot Arm 2 end: [8.79842576458462;8.91794201075106;0.662843861048644]
Robot Arm 1 togoal: 26.6834363062099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.663077143270097;8.89938398985081;9.5388065001269]
Robot Arm 2 end: [8.77073998211627;8.72573005523839;0.667271585072598]
Robot Arm 1 togoal: 26.5277684282778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998817e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.927598522620039;8.99124916268464;9.44730998551536]
Robot Arm 2 end: [8.74521278691915;8.65402887579979;0.805423857212416]
Robot Arm 1 togoal: 26.4550154969181

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-0.989570072520306;8.98399246583972;9.26714738834029]
Robot Arm 2 end: [8.84711635460308;8.45184837799518;0.86988643110959]
Robot Arm 1 togoal: 26.3838672739245

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.17889637397676;9.07147127722096;9.20547456870691]
Robot Arm 2 end: [8.86409064941913;8.45941421633269;1.01395638637075]
Robot Arm 1 togoal: 26.3981246013002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.39078140800718;9.1641108557896;9.07855307402133]
Robot Arm 2 end: [8.95483219066595;8.43244879608473;1.14387278510411]
Robot Arm 1 togoal: 26.4420698307225

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.28288305931389;9.11851093869027;8.90338571286056]
Robot Arm 2 end: [8.85198272380029;8.157411505505;0.96726639239107]
Robot Arm 1 togoal: 26.180058558126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.49907690560605;9.21874768870227;8.83502626776466]
Robot Arm 2 end: [9.05947156619715;8.17474207867169;1.14214664580812]
Robot Arm 1 togoal: 26.3387212269863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.314616e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.52319552683669;9.2452342486852;8.66521197443649]
Robot Arm 2 end: [9.10592548116627;8.03806310890848;1.18744479328048]
Robot Arm 1 togoal: 26.2777352199304
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.47914250828901;9.24388933787302;8.52311082089344]
Robot Arm 2 end: [9.00324048408769;7.87261793548514;1.10131642114205]
Robot Arm 1 togoal: 26.0907982834994
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.46956968228017;9.24033616048527;8.34204265316803]
Robot Arm 2 end: [9.00770313229014;7.71671840749481;1.1443243437208]
Robot Arm 1 togoal: 25.9868577178114
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.40550555224611;9.21750648512196;8.19072703365232]
Robot Arm 2 end: [8.87761919343953;7.56612376577102;1.05839315539293]
Robot Arm 1 togoal: 25.7904481477073
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.48232745742509;9.23289859902228;8.00517519836431]
Robot Arm 2 end: [8.99477886338893;7.39207439428281;1.12673851873354]
Robot Arm 1 togoal: 25.7573546374802
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-1.39189186581344;9.1894799190258;7.84931507748669]
Robot Arm 2 end: [8.85054057873703;7.23821730687296;1.04608569287221]
Robot Arm 1 togoal: 25.5484747025865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.773461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-1.5985651582682;9.25952732175437;7.70589418836157]
Robot Arm 2 end: [9.02511977723992;7.11157680394402;1.1089670206487]
Robot Arm 1 togoal: 25.5916200040496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.840693e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-1.40799647734226;9.14556920496882;7.55277463744518]
Robot Arm 2 end: [8.82160904546322;6.95067814880752;1.03747836121762]
Robot Arm 1 togoal: 25.334013695818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996612e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-1.652231431384;9.23018744358737;7.45787338222131]
Robot Arm 2 end: [8.99037468515857;6.80517765205519;1.01673926751557]
Robot Arm 1 togoal: 25.3634107620835

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003927e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-1.60548307166964;9.15528617503781;7.26604406136726]
Robot Arm 2 end: [8.8486648709298;6.61485211022403;0.937327511656201]
Robot Arm 1 togoal: 25.1329694153892

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-1.73477439935807;9.14771188237941;7.10213082610906]
Robot Arm 2 end: [8.94087117358214;6.40524260329064;0.957357614987767]
Robot Arm 1 togoal: 25.0640435370121

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011288e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-1.60764892062033;9.03073372878872;6.91905898428633]
Robot Arm 2 end: [8.71362245420073;6.2477292428358;0.875357883406764]
Robot Arm 1 togoal: 24.7912328182684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.836433e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-1.73776605547587;9.02325553003126;6.7572943621575]
Robot Arm 2 end: [8.69264818271644;6.06963200772293;0.849344502072599]
Robot Arm 1 togoal: 24.6596693013986

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.019638e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-1.58049999811408;8.87410596708238;6.5798521894789]
Robot Arm 2 end: [8.4346489632875;5.93773388343576;0.765222696975008]
Robot Arm 1 togoal: 24.3807063693942

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.781648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-1.71059093305705;8.8491514464488;6.41762087359775]
Robot Arm 2 end: [8.46427119641243;5.73468474439281;0.78284762937169]
Robot Arm 1 togoal: 24.2704446548133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001318e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-1.59448123428988;8.70759041290358;6.23257299500956]
Robot Arm 2 end: [8.21829362247752;5.60922212761862;0.696325213939974]
Robot Arm 1 togoal: 24.0048771387589

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.768512e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-1.69333546668577;8.65055925604508;6.06352043119622]
Robot Arm 2 end: [8.23723245769687;5.38333296418686;0.74698158735942]
Robot Arm 1 togoal: 23.8713779515598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-1.59481327219662;8.50920142355397;5.87498270294494]
Robot Arm 2 end: [7.99632531415443;5.26099752016655;0.658610845175688]
Robot Arm 1 togoal: 23.6116387651887

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-1.6885019749725;8.43944100180762;5.70395898326365]
Robot Arm 2 end: [8.02285461271736;5.01987153900423;0.748531711579369]
Robot Arm 1 togoal: 23.4737414294346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-1.57220604661901;8.28599754858145;5.51583774503694]
Robot Arm 2 end: [7.77072050715314;4.89604764816909;0.659199751797442]
Robot Arm 1 togoal: 23.2041686459067

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [-1.69598015865849;8.23105544284203;5.34784391099484]
Robot Arm 2 end: [7.81388736061495;4.65997390778455;0.749398200901816]
Robot Arm 1 togoal: 23.0834310103583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-1.55096265120641;8.06104576513165;5.16454985899431]
Robot Arm 2 end: [7.54562890085993;4.53879139485171;0.659949301599673]
Robot Arm 1 togoal: 22.8027839759737

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.647849e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-1.70743146404205;8.0246449581127;5.00369766141482]
Robot Arm 2 end: [7.60847306191316;4.31148937777776;0.750038581283096]
Robot Arm 1 togoal: 22.7039959762927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.557946e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-1.5371752871374;7.8439069276937;4.83087043467227]
Robot Arm 2 end: [7.33021824837729;4.19869589310907;0.66106579403267]
Robot Arm 1 togoal: 22.4205021451344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.444068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-1.71014407239456;7.81991556059714;4.67853518885906]
Robot Arm 2 end: [7.40542035633519;3.97879953480696;0.751805762076148]
Robot Arm 1 togoal: 22.3371250162303

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.488148e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.908577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307231628986;-6.24962959904462;-7.10439671995214]
Robot Arm 1 end: [-6.13860293103653;-6.93244482188652;1.24046684503414]
Robot Arm 1 togoal: 23.3931804392419
Mobile Platform 2 Pose: [-1.52225381433418;7.63211833523076;4.51351298639414]
Robot Arm 2 end: [7.12294002837057;3.87519864845651;0.662607897015328]
Robot Arm 1 togoal: 22.055680668521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.682130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407584898819;-6.21793451996414;-7.10337760756621]
Robot Arm 1 end: [-6.10318314321659;-6.8809473344603;1.17439732471865]
Robot Arm 1 togoal: 23.3323600106657
Mobile Platform 2 Pose: [-1.66554637006841;7.59116756644091;4.35931769518739]
Robot Arm 2 end: [7.17973535468231;3.64305513924801;0.708044588098758]
Robot Arm 1 togoal: 21.9530671408626

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.967603e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.6685928490436;-6.18978096834309;-7.1021914895646]
Robot Arm 1 end: [-6.07852494652395;-6.82865593821891;1.10279061829263]
Robot Arm 1 togoal: 23.2786924939931
Mobile Platform 2 Pose: [-1.48160737754607;7.40434324438678;4.19319327262347]
Robot Arm 2 end: [6.9012527249478;3.55171978935598;0.617020427964777]
Robot Arm 1 togoal: 21.6822192588405

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000063e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70845565512298;-6.16355502860569;-7.10107998190879]
Robot Arm 1 end: [-6.05860434219163;-6.77639184213971;1.02304643240352]
Robot Arm 1 togoal: 23.2287333794925
Mobile Platform 2 Pose: [-1.52326541062762;7.30649725327203;4.0276293576942]
Robot Arm 2 end: [6.90850608936653;3.30054096888848;0.663526290365372]
Robot Arm 1 togoal: 21.5299147560305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000231e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74536587892517;-6.1381585944645;-7.10000571240084]
Robot Arm 1 end: [-6.04160576362502;-6.72126421519155;0.930981985961948]
Robot Arm 1 togoal: 23.1793423911616
Mobile Platform 2 Pose: [-1.39203219246083;7.15520853936976;3.86013955390373]
Robot Arm 2 end: [6.65885006749657;3.21772020564877;0.618907245932714]
Robot Arm 1 togoal: 21.2847171544065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78013629357187;-6.11348816295203;-7.09883215712233]
Robot Arm 1 end: [-6.02898079997672;-6.65919937854948;0.819178160407769]
Robot Arm 1 togoal: 23.1289797292231
Mobile Platform 2 Pose: [-1.3749103948209;7.03117183280144;3.69476742695011]
Robot Arm 2 end: [6.64785968664503;2.96740355561156;0.674422456400879]
Robot Arm 1 togoal: 21.1192206451288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977731e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963948e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80726348426166;-6.09290685057346;-7.09790755367426]
Robot Arm 1 end: [-5.99728249608177;-6.60133339565518;0.729762471431997]
Robot Arm 1 togoal: 23.0682330185854
Mobile Platform 2 Pose: [-1.26007637813141;6.89642750124741;3.53173836468179]
Robot Arm 2 end: [6.50637482843072;2.82728236242248;0.642242125760738]
Robot Arm 1 togoal: 20.9229968738588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983353e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.826142e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82888573446245;-6.07527493527145;-7.09843223379835]
Robot Arm 1 end: [-5.97791015222349;-6.56348227936463;0.646703112163324]
Robot Arm 1 togoal: 23.0305656269974
Mobile Platform 2 Pose: [-1.17115791485068;6.76713886728005;3.36807507961713]
Robot Arm 2 end: [6.49786004081318;2.61169666229512;0.728941289493361]
Robot Arm 1 togoal: 20.7812808948076

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987304e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.876934e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85475178814521;-6.05587603487667;-7.10073896827715]
Robot Arm 1 end: [-5.94861343946309;-6.53413019465589;0.572706660278535]
Robot Arm 1 togoal: 22.9920636784395
Mobile Platform 2 Pose: [-1.14478192495404;6.69900157703971;3.2095068545005]
Robot Arm 2 end: [6.58392814834076;2.41622913082968;0.644860963826511]
Robot Arm 1 togoal: 20.7354054281391

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977925e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89499266082782;-6.02930330597339;-7.10425618327397]
Robot Arm 1 end: [-5.91362009471157;-6.51027357818143;0.511715831061209]
Robot Arm 1 togoal: 22.9532370849707
Mobile Platform 2 Pose: [-1.01579222393188;6.60247218276071;3.06197814917619]
Robot Arm 2 end: [6.46774210801897;2.28587199275861;0.728177088206457]
Robot Arm 1 togoal: 20.5610736326714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000283e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.9451459339488;-5.99844156246739;-7.10839302603073]
Robot Arm 1 end: [-5.88435634230984;-6.49003338428611;0.462760005235337]
Robot Arm 1 togoal: 22.9205987991341
Mobile Platform 2 Pose: [-1.07972341072317;6.59356436508205;2.91380965271209]
Robot Arm 2 end: [6.61092795721084;2.11682676835087;0.646720089082916]
Robot Arm 1 togoal: 20.5792318587577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.9971816974244;-5.96797660435409;-7.11341912065872]
Robot Arm 1 end: [-5.8581985804872;-6.47340432933922;0.424887688897148]
Robot Arm 1 togoal: 22.8922907244908
Mobile Platform 2 Pose: [-0.995430507936521;6.53033115420583;2.76570552132869]
Robot Arm 2 end: [6.61857181048656;1.98063914797971;0.72986698703369]
Robot Arm 1 togoal: 20.5021592730768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967196e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000083e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.04721854620549;-5.94054855084073;-7.11958197409411]
Robot Arm 1 end: [-5.80816348288738;-6.46958886428302;0.388981892766925]
Robot Arm 1 togoal: 22.8566588777825
Mobile Platform 2 Pose: [-1.05816710439916;6.54591087180365;2.62621386116989]
Robot Arm 2 end: [6.78472537081943;1.86145527209229;0.648166333515351]
Robot Arm 1 togoal: 20.5714113651695

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.744215e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.950160e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.0793200521793;-5.92458810946839;-7.12413520024817]
Robot Arm 1 end: [-5.75993797822634;-6.47597520024021;0.373788310908031]
Robot Arm 1 togoal: 22.8286990110269
Mobile Platform 2 Pose: [-0.998877333692356;6.51927516980002;2.48841888449984]
Robot Arm 2 end: [6.86731642198147;1.76848491826253;0.706758395343093]
Robot Arm 1 togoal: 20.5831627778962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.825888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.11986523914492;-5.89784017882743;-7.12347756525887]
Robot Arm 1 end: [-5.76042045546424;-6.48314270655142;0.379799277827377]
Robot Arm 1 togoal: 22.833904298342
Mobile Platform 2 Pose: [-1.05345162588572;6.55703292520226;2.36108880724955]
Robot Arm 2 end: [7.03974057903219;1.71335552107507;0.624659750280608]
Robot Arm 1 togoal: 20.6968326660647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.396544e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997812e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.16510057129898;-5.87020909775997;-7.12399667632486]
Robot Arm 1 end: [-5.64288514963426;-6.553219269185;0.346217898925177]
Robot Arm 1 togoal: 22.8054606829893
Mobile Platform 2 Pose: [-1.00664370303812;6.56219712512153;2.24024767722765]
Robot Arm 2 end: [7.14469596805705;1.67561815820275;0.642682066844735]
Robot Arm 1 togoal: 20.761312617582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.703907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996756e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.19727722921923;-5.84995661268323;-7.12835655710684]
Robot Arm 1 end: [-5.65577895006618;-6.55949046778415;0.34812042204152]
Robot Arm 1 togoal: 22.8187595103319
Mobile Platform 2 Pose: [-1.0158880453442;6.60435007720666;2.12701717671884]
Robot Arm 2 end: [7.28074586059351;1.68541457803914;0.559814375412498]
Robot Arm 1 togoal: 20.8829348790821

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.027447e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.22057193660345;-5.83489475375421;-7.13209482806456]
Robot Arm 1 end: [-5.67744711239261;-6.56867762354305;0.333392263487225]
Robot Arm 1 togoal: 22.8410559801991
Mobile Platform 2 Pose: [-0.939948321689228;6.61967344213621;2.04041490448762]
Robot Arm 2 end: [7.32939919623617;1.65148642125339;0.56473646733358]
Robot Arm 1 togoal: 20.904058272763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.561282e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000063e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.24035190156889;-5.82219533142941;-7.13527608565516]
Robot Arm 1 end: [-5.72209575727311;-6.57914956158611;0.315040522102079]
Robot Arm 1 togoal: 22.8802717083657
Mobile Platform 2 Pose: [-0.858430708024748;6.63644376078346;1.96319431492451]
Robot Arm 2 end: [7.35312151362075;1.6143262249718;0.505199480029284]
Robot Arm 1 togoal: 20.9058664164614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000267e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.2578706605066;-5.81109956948411;-7.13781077732004]
Robot Arm 1 end: [-5.7681417089048;-6.59297656899627;0.293133491023057]
Robot Arm 1 togoal: 22.9231066946701
Mobile Platform 2 Pose: [-0.742527297249129;6.63269933068933;1.91588913375637]
Robot Arm 2 end: [7.31829223922682;1.56596776243009;0.40041955863807]
Robot Arm 1 togoal: 20.8554682731028

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.914667e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000470e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.27290266796922;-5.80153145089411;-7.13996951784898]
Robot Arm 1 end: [-5.81734513309049;-6.61021455017859;0.270042110231057]
Robot Arm 1 togoal: 22.9706884986985
Mobile Platform 2 Pose: [-0.613522835127008;6.61615001347408;1.88240467283415]
Robot Arm 2 end: [7.21901333576729;1.49116559944693;0.279959907422922]
Robot Arm 1 togoal: 20.7384061008224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949391e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.28669643860547;-5.79269858115781;-7.14190246213186]
Robot Arm 1 end: [-5.88854373085104;-6.64170417266389;0.24699700805434]
Robot Arm 1 togoal: 23.0437675583033
Mobile Platform 2 Pose: [-0.457861974086448;6.58912096225561;1.84348223977106]
Robot Arm 2 end: [7.05475109962465;1.40020527122543;0.158127814129267]
Robot Arm 1 togoal: 20.5593442475981

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.29837929714572;-5.78521358220947;-7.14338388612782]
Robot Arm 1 end: [-5.95522065652472;-6.68140648570887;0.223498357250767]
Robot Arm 1 togoal: 23.1197296492148
Mobile Platform 2 Pose: [-0.272231172436842;6.55230219697921;1.7826677244243]
Robot Arm 2 end: [6.92969376643682;1.25333256167572;0.164746443433413]
Robot Arm 1 togoal: 20.3737931784609

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.31085638177914;-5.77720171731978;-7.14476567172649]
Robot Arm 1 end: [-6.04464209923801;-6.72813565983406;0.218057715082151]
Robot Arm 1 togoal: 23.2154845166815
Mobile Platform 2 Pose: [-0.0806089728598094;6.50017004462349;1.71471217018729]
Robot Arm 2 end: [6.76345153039507;1.12783933677735;0.154568652416377]
Robot Arm 1 togoal: 20.1670337468031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956164e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996738e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.32341556317938;-5.76919511902487;-7.14587823532478]
Robot Arm 1 end: [-6.1385715770276;-6.77890311504729;0.232044944123913]
Robot Arm 1 togoal: 23.3162279845402
Mobile Platform 2 Pose: [0.0956300913566785;6.45041053977179;1.62423664823178]
Robot Arm 2 end: [6.60401998493367;0.984145588044868;0.171542619066773]
Robot Arm 1 togoal: 19.9540965513951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987278e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000171e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.34303901993991;-5.75662512930764;-7.14735569759422]
Robot Arm 1 end: [-6.17602602067729;-6.77302741394044;0.289773798871838]
Robot Arm 1 togoal: 23.3348277132866
Mobile Platform 2 Pose: [0.216182535179608;6.41923551510395;1.50705538039559]
Robot Arm 2 end: [6.43527170221728;0.873061839347369;0.134385565996011]
Robot Arm 1 togoal: 19.7551089411464

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982427e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.35790535982354;-5.7469197968001;-7.14863449312205]
Robot Arm 1 end: [-6.21751442146658;-6.77317760302378;0.345194120405949]
Robot Arm 1 togoal: 23.3608600094162
Mobile Platform 2 Pose: [0.278506945085648;6.39677896046842;1.37407275397314]
Robot Arm 2 end: [6.30338519661102;0.711841238073707;0.186188598590745]
Robot Arm 1 togoal: 19.5531489294207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979824e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.889046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.37069229957908;-5.73833160260283;-7.14987570906945]
Robot Arm 1 end: [-6.25802278367258;-6.78189513628627;0.399085757955772]
Robot Arm 1 togoal: 23.3926056856911
Mobile Platform 2 Pose: [0.228781372206825;6.40346604884053;1.22387640320878]
Robot Arm 2 end: [6.38180673099012;0.58275789957487;0.131287301078336]
Robot Arm 1 togoal: 19.5522289756966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992324e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.462405e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.38213497937254;-5.73033276822527;-7.15117237916443]
Robot Arm 1 end: [-6.24719042767381;-6.82400393140463;0.330008391078078]
Robot Arm 1 togoal: 23.4187268246917
Mobile Platform 2 Pose: [0.287734110182192;6.3680668892311;1.069942400587]
Robot Arm 2 end: [6.23687895379349;0.408743952157261;0.183895825607584]
Robot Arm 1 togoal: 19.3330296744946

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.4126073465199;-5.71136844070097;-7.1533442946416]
Robot Arm 1 end: [-6.21979384983282;-6.87025635998318;0.281779637197676]
Robot Arm 1 togoal: 23.4355057400136
Mobile Platform 2 Pose: [0.246877817231794;6.35134158582001;0.899980877441554]
Robot Arm 2 end: [6.3263372051003;0.227887889742003;0.162635111768891]
Robot Arm 1 togoal: 19.3133148990367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996287e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.42935924442145;-5.69936124176163;-7.15592571953079]
Robot Arm 1 end: [-6.19954406960077;-6.92326548346581;0.244815252090249]
Robot Arm 1 togoal: 23.4616877099155
Mobile Platform 2 Pose: [0.336867458902318;6.29309452575822;0.740377530551959]
Robot Arm 2 end: [6.16072755927539;0.0736749021480659;0.181919062168205]
Robot Arm 1 togoal: 19.0903425594554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996930e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.44052837018541;-5.69119865441199;-7.15834559881726]
Robot Arm 1 end: [-6.19087333749361;-6.98208872355608;0.215495341318366]
Robot Arm 1 togoal: 23.4997884939276
Mobile Platform 2 Pose: [0.312327639678784;6.26095237697765;0.562361804172972]
Robot Arm 2 end: [6.22771196747417;-0.123210919965016;0.172776770748074]
Robot Arm 1 togoal: 19.0448760591057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.45190978028909;-5.68381149664716;-7.16033260433286]
Robot Arm 1 end: [-6.20291130866594;-7.04536988276052;0.206163937735757]
Robot Arm 1 togoal: 23.5543576262917
Mobile Platform 2 Pose: [0.40870508446903;6.19412082952106;0.401470712395366]
Robot Arm 2 end: [6.05413950439977;-0.269218550716095;0.180489941544542]
Robot Arm 1 togoal: 18.8207457359499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.46285261789065;-5.6772912389928;-7.16180404957069]
Robot Arm 1 end: [-6.22201626605093;-7.09946679066566;0.216281035499567]
Robot Arm 1 togoal: 23.6060981070025
Mobile Platform 2 Pose: [0.351644954821491;6.1718118423556;0.223755338990313]
Robot Arm 2 end: [6.13651903604803;-0.475878600677733;0.188993528363103]
Robot Arm 1 togoal: 18.7848443238226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999279e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.46950774884438;-5.67396828711733;-7.16259677629618]
Robot Arm 1 end: [-6.24733136003678;-7.14461728140969;0.236619343977331]
Robot Arm 1 togoal: 23.6550906089664
Mobile Platform 2 Pose: [0.488925387882898;6.08119586478131;0.0669991622209007]
Robot Arm 2 end: [5.9404918846759;-0.608264283185856;0.179559189866699]
Robot Arm 1 togoal: 18.550025464861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999426e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.46739939570012;-5.67663977119019;-7.16229045028814]
Robot Arm 1 end: [-6.28326051738443;-7.17724750224422;0.263015257505932]
Robot Arm 1 togoal: 23.7020026673426
Mobile Platform 2 Pose: [0.404743233606726;6.07072613487866;-0.106248118208046]
Robot Arm 2 end: [6.03394708913313;-0.817058602716905;0.20357244596598]
Robot Arm 1 togoal: 18.5242945162072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.329613e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.45259672432864;-5.68817827869891;-7.15969388314308]
Robot Arm 1 end: [-6.33725878470889;-7.18822154920921;0.293988432240161]
Robot Arm 1 togoal: 23.7454530807619
Mobile Platform 2 Pose: [0.565521929490423;5.96690203314164;-0.257113593706449]
Robot Arm 2 end: [5.8332578107675;-0.900638511087251;0.128512863640082]
Robot Arm 1 togoal: 18.3147332176329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000785e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.41852014232805;-5.71359638400193;-7.15100284968974]
Robot Arm 1 end: [-6.39403502190872;-7.16286088919745;0.297755837861288]
Robot Arm 1 togoal: 23.7660367853313
Mobile Platform 2 Pose: [0.442386391622472;5.97275177467522;-0.4265344558061]
Robot Arm 2 end: [5.93641549425582;-1.11913271265518;0.170907086286967]
Robot Arm 1 togoal: 18.2937665191356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.37341486013679;-5.750947959208;-7.12845828563763]
Robot Arm 1 end: [-6.43297010523534;-7.11630621180148;0.263230235454961]
Robot Arm 1 togoal: 23.761153793457
Mobile Platform 2 Pose: [0.629763647688858;5.85297376398241;-0.574515432154952]
Robot Arm 2 end: [5.72406810186415;-1.19808394355151;0.0974912162051803]
Robot Arm 1 togoal: 18.0761284902041

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001685e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.31974878218977;-5.80078334227166;-7.08408424598204]
Robot Arm 1 end: [-6.4680319372756;-7.03956298632743;0.222978953279184]
Robot Arm 1 togoal: 23.7324175188681
Mobile Platform 2 Pose: [0.480197334050326;5.8676291878483;-0.743145635589773]
Robot Arm 2 end: [5.83157573094872;-1.42437654413297;0.149909764205402]
Robot Arm 1 togoal: 18.0571314503792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960966e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-4.22561114711574;-5.87915625541638;-7.01687613402964]
Robot Arm 1 end: [-6.56749378527981;-6.92394002389701;0.26092480785561]
Robot Arm 1 togoal: 23.7168586821191
Mobile Platform 2 Pose: [0.691723403504567;5.73504771362896;-0.884132421206547]
Robot Arm 2 end: [5.60985481318841;-1.49577935797587;0.0776122715346525]
Robot Arm 1 togoal: 17.8345929068127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001760e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-4.1645742941419;-5.94046700365837;-6.9212893266127]
Robot Arm 1 end: [-6.63597085456265;-6.81949824355437;0.269780711978356]
Robot Arm 1 togoal: 23.6900104927268
Mobile Platform 2 Pose: [0.544454592146378;5.74609702681126;-1.05310218003489]
Robot Arm 2 end: [5.71048667384878;-1.72704808051422;0.133727245492555]
Robot Arm 1 togoal: 17.8097066423538

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.563889e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-4.21267016742043;-5.93837562304526;-6.82252368207165]
Robot Arm 1 end: [-6.62980474769538;-6.74278040996567;0.248630211563211]
Robot Arm 1 togoal: 23.6323935876442
Mobile Platform 2 Pose: [0.763029329485356;5.61102475469045;-1.18720081375933]
Robot Arm 2 end: [5.48624893198719;-1.80557133148624;0.0794144336148671]
Robot Arm 1 togoal: 17.5798389108673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000741e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-4.27825041000458;-5.92363626784472;-6.72155556052954]
Robot Arm 1 end: [-6.6196194747532;-6.63419638509398;0.250591815579304]
Robot Arm 1 togoal: 23.5482945840779
Mobile Platform 2 Pose: [0.639523007556492;5.61174395695344;-1.35484355714931]
Robot Arm 2 end: [5.57596536988456;-2.03394324568753;0.134357508549726]
Robot Arm 1 togoal: 17.5496875988112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.734074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-4.43309280656152;-5.84906128939113;-6.64074870677323]
Robot Arm 1 end: [-6.52272618266714;-6.57546607435506;0.203273877463658]
Robot Arm 1 togoal: 23.4410528394845
Mobile Platform 2 Pose: [0.842919761892052;5.48600365350026;-1.48498725458594]
Robot Arm 2 end: [5.35984019375433;-2.11891301494631;0.0931438574545013]
Robot Arm 1 togoal: 17.3226728886583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-4.59594165580353;-5.76703948378749;-6.55560764293439]
Robot Arm 1 end: [-6.44203906183518;-6.43415233129237;0.212930319353342]
Robot Arm 1 togoal: 23.2837509359739
Mobile Platform 2 Pose: [0.739347542599945;5.48091623504657;-1.64725581109514]
Robot Arm 2 end: [5.44474002003813;-2.33861707798476;0.147584839601785]
Robot Arm 1 togoal: 17.2951742335001

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-4.77387577102585;-5.6700545775996;-6.51606577453307]
Robot Arm 1 end: [-6.33733577798534;-6.42176508642478;0.194350316915904]
Robot Arm 1 togoal: 23.2022177443879
Mobile Platform 2 Pose: [0.920041925653932;5.36769653362707;-1.77592829981055]
Robot Arm 2 end: [5.23964961812915;-2.42539331724489;0.110201694962695]
Robot Arm 1 togoal: 17.0766489317805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997742e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-4.94158483418481;-5.58044644625203;-6.47955967565452]
Robot Arm 1 end: [-6.18593160300386;-6.39912850695021;0.11150902041141]
Robot Arm 1 togoal: 23.0846802547951
Mobile Platform 2 Pose: [0.82067580804435;5.3637208415848;-1.93232264589595]
Robot Arm 2 end: [5.32684251381318;-2.63703745289939;0.166360361246392]
Robot Arm 1 togoal: 17.0575604194773

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962360e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-5.16142542806287;-5.46819779888196;-6.41933742699631]
Robot Arm 1 end: [-6.08490270640965;-6.31037372183659;0.133250508950516]
Robot Arm 1 togoal: 22.949471337496
Mobile Platform 2 Pose: [0.985146397375183;5.25871127847521;-2.06038741142975]
Robot Arm 2 end: [5.12166117387569;-2.7582727631996;0.178177930018946]
Robot Arm 1 togoal: 16.8199411753235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989376e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-5.36978751726263;-5.36512280804269;-6.38824870485]
Robot Arm 1 end: [-6.04415177920959;-6.19484357690056;0.258733964727169]
Robot Arm 1 togoal: 22.8315807674019
Mobile Platform 2 Pose: [0.90985468979501;5.24700809180799;-2.20988614919132]
Robot Arm 2 end: [5.20782035480614;-2.95604015409215;0.237474330270607]
Robot Arm 1 togoal: 16.8090037896263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999706e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-5.43720835581599;-5.3332596507173;-6.38207581758679]
Robot Arm 1 end: [-5.99178948768168;-6.18506638531721;0.231000298357031]
Robot Arm 1 togoal: 22.7894230761924
Mobile Platform 2 Pose: [1.04148659769333;5.15940687732587;-2.33551001109368]
Robot Arm 2 end: [5.02304224742797;-3.03137601635293;0.177969334383672]
Robot Arm 1 togoal: 16.6149706578812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996177e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-5.49372806540615;-5.30783720738767;-6.37563530953874]
Robot Arm 1 end: [-5.94867042021907;-6.15380688954483;0.223820389732838]
Robot Arm 1 togoal: 22.7373754012808
Mobile Platform 2 Pose: [0.952039587644663;5.15498457811906;-2.48267077418964]
Robot Arm 2 end: [5.11542445559336;-3.22715946037563;0.238751053031649]
Robot Arm 1 togoal: 16.6129945467375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-5.54638910370612;-5.28540628145535;-6.3692835081264]
Robot Arm 1 end: [-5.90436018828796;-6.10646888137464;0.225950041517998]
Robot Arm 1 togoal: 22.6725421428752
Mobile Platform 2 Pose: [1.0824369508455;5.0663162478366;-2.61046915049798]
Robot Arm 2 end: [4.9299829791501;-3.30444211163945;0.177859941915875]
Robot Arm 1 togoal: 16.4176402814086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-5.60147304690187;-5.26492462813518;-6.36116227890772]
Robot Arm 1 end: [-5.84987355974324;-6.03890485561673;0.233705361349544]
Robot Arm 1 togoal: 22.5858825002273
Mobile Platform 2 Pose: [0.984599056556055;5.0640013630335;-2.75788170294657]
Robot Arm 2 end: [5.02353222861849;-3.50225343713378;0.242245914417398]
Robot Arm 1 togoal: 16.4183568919357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-5.63513144765535;-5.26824046670881;-6.34478529078144]
Robot Arm 1 end: [-5.80730233558141;-5.98019626328206;0.221130619314021]
Robot Arm 1 togoal: 22.5150425304581
Mobile Platform 2 Pose: [1.11888280542333;4.97156803345383;-2.88626231178064]
Robot Arm 2 end: [4.83516835629531;-3.57916736988129;0.177821383016419]
Robot Arm 1 togoal: 16.2207718873284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.472351e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000002e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-5.59182380319916;-5.31389969770402;-6.32932252192387]
Robot Arm 1 end: [-5.77139574151027;-6.0534058605969;0.0847482597475327]
Robot Arm 1 togoal: 22.5502013831751
Mobile Platform 2 Pose: [1.01652423814642;4.96941032333173;-3.03385379316806]
Robot Arm 2 end: [4.92806364388883;-3.77909459198718;0.246178692789205]
Robot Arm 1 togoal: 16.2225743215051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.813264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.861509e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-5.49260043809197;-5.42231039373265;-6.28401416957094]
Robot Arm 1 end: [-5.8659969234884;-6.02936225031225;0.0658122047947392]
Robot Arm 1 togoal: 22.6006168952795
Mobile Platform 2 Pose: [1.15188657190243;4.87510969696005;-3.16150166088236]
Robot Arm 2 end: [4.74186570606965;-3.85169033001828;0.17746759311801]
Robot Arm 1 togoal: 16.0290173005748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.281570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.931130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-5.47978151133958;-5.42882887492934;-6.28527113223219]
Robot Arm 1 end: [-5.86030302696182;-5.96953055944565;0.0880652689697652]
Robot Arm 1 togoal: 22.5527918524407
Mobile Platform 2 Pose: [1.13942136600862;4.8261623997793;-3.32173446959811]
Robot Arm 2 end: [4.80041825412753;-3.99264439955823;0.127429701450068]
Robot Arm 1 togoal: 16.0337999234202

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.938449e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.775661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-5.48493443149701;-5.42180915448574;-6.28812133000935]
Robot Arm 1 end: [-5.80452157906429;-5.87249481481439;0.122343260463143]
Robot Arm 1 togoal: 22.4427100561512
Mobile Platform 2 Pose: [1.31441556067849;4.72386127163313;-3.43048876955682]
Robot Arm 2 end: [4.5685010279224;-4.02914927241615;0.0793628535203485]
Robot Arm 1 togoal: 15.8104585092835

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.291569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.766008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-5.45649225607339;-5.42955289189229;-6.29361322183389]
Robot Arm 1 end: [-5.75535062020001;-5.74172559713596;0.221553509592151]
Robot Arm 1 togoal: 22.3096741151097
Mobile Platform 2 Pose: [1.31416660594906;4.67180466336039;-3.58657200303314]
Robot Arm 2 end: [4.37384970704117;-4.09363554898641;0.0707178519266673]
Robot Arm 1 togoal: 15.6075627417552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.223766e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.039380e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-5.47482416692983;-5.4140294239726;-6.29985851596612]
Robot Arm 1 end: [-5.62016729241777;-5.70549476040362;0.203900906285603]
Robot Arm 1 togoal: 22.1897936433588
Mobile Platform 2 Pose: [0.947745138631551;4.76343409932489;-3.70842806739911]
Robot Arm 2 end: [4.40359946871246;-4.17969722933301;0.127202672810636]
Robot Arm 1 togoal: 15.5975085506182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.418778e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-5.58130594265986;-5.34701986564878;-6.31961430899737]
Robot Arm 1 end: [-5.42360882793165;-5.69715687451942;0.204847711261879]
Robot Arm 1 togoal: 22.0458771671139
Mobile Platform 2 Pose: [0.86302080224394;4.68336630795921;-3.83500714062314]
Robot Arm 2 end: [4.22981515555462;-4.26511966542101;0.187991754101798]
Robot Arm 1 togoal: 15.3997858036021

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.043014e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-5.68061492698035;-5.28825236374661;-6.32490526617398]
Robot Arm 1 end: [-5.22898525728791;-5.6956425066178;0.212188788852327]
Robot Arm 1 togoal: 21.9086323728494
Mobile Platform 2 Pose: [0.979879286779749;4.46668379276;-3.95970151742877]
Robot Arm 2 end: [3.95156383482625;-4.24864665835707;0.15354262428936]
Robot Arm 1 togoal: 15.1523641182505

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-5.77109784909959;-5.23529836886739;-6.32596970905442]
Robot Arm 1 end: [-5.03607987800636;-5.70711724641592;0.241051504783177]
Robot Arm 1 togoal: 21.7815403137807
Mobile Platform 2 Pose: [1.14252345637091;4.22351516165641;-4.01774475000001]
Robot Arm 2 end: [3.66375359013692;-4.2678362146053;0.190650808195837]
Robot Arm 1 togoal: 14.877018948926

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-5.85733101472154;-5.1846610831037;-6.32535562872891]
Robot Arm 1 end: [-4.85076713013467;-5.7330278177415;0.289183352166859]
Robot Arm 1 togoal: 21.6700443220865
Mobile Platform 2 Pose: [1.41544776195393;3.97499196072903;-4.04487809851703]
Robot Arm 2 end: [3.34845362688095;-4.24989488670647;0.207720509552814]
Robot Arm 1 togoal: 14.5934788882893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-5.94390212059379;-5.13326957141296;-6.32377074703325]
Robot Arm 1 end: [-4.81920939098895;-5.70238134890105;0.300197698929069]
Robot Arm 1 togoal: 21.6255428893378
Mobile Platform 2 Pose: [1.69286683587222;3.72538941136599;-4.05896949136505]
Robot Arm 2 end: [3.07540593151349;-4.0816274062546;0.163126282175107]
Robot Arm 1 togoal: 14.4165597652774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.786033e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-6.01629572720578;-5.09046242704952;-6.32081163841917]
Robot Arm 1 end: [-4.80371295486835;-5.64910092157266;0.335179020664682]
Robot Arm 1 togoal: 21.5743006444875
Mobile Platform 2 Pose: [1.45149516019808;3.57163169014715;-4.05116039450009]
Robot Arm 2 end: [2.93125188043357;-4.22182614618897;0.212677217067318]
Robot Arm 1 togoal: 14.2237926301118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-6.05850671614802;-5.06616925078944;-6.31801762752068]
Robot Arm 1 end: [-4.71381351768264;-5.60514792752996;0.482750585004703]
Robot Arm 1 togoal: 21.4731206842641
Mobile Platform 2 Pose: [1.49802835293672;3.32269028485867;-4.07414249065952]
Robot Arm 2 end: [2.70400577361541;-4.26689129297351;0.159881074979533]
Robot Arm 1 togoal: 14.0040330930302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806439e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-6.04199037615881;-5.07129517933734;-6.31508123820247]
Robot Arm 1 end: [-4.91162494282685;-5.50699673387938;0.673381161572003]
Robot Arm 1 togoal: 21.5300247058692
Mobile Platform 2 Pose: [1.25504293926897;3.16741388148129;-4.07885471069546]
Robot Arm 2 end: [2.54053221015567;-4.34855796390663;0.244907953798885]
Robot Arm 1 togoal: 13.814201247689

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995698e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-6.09970618764054;-5.03731718266696;-6.30790238295853]
Robot Arm 1 end: [-5.01238198496027;-5.49444847234246;0.809756007661484]
Robot Arm 1 togoal: 21.5860004248372
Mobile Platform 2 Pose: [1.27076615062045;2.91976216899242;-4.11303993130172]
Robot Arm 2 end: [2.36495386635314;-4.44597010468593;0.15853941501506]
Robot Arm 1 togoal: 13.6233487454708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-6.11333670637567;-5.00762513005862;-6.23902664656788]
Robot Arm 1 end: [-5.06857007265057;-5.42852907325964;0.903143469965676]
Robot Arm 1 togoal: 21.5750564330781
Mobile Platform 2 Pose: [1.03671586785433;2.77400320065778;-4.14449789756132]
Robot Arm 2 end: [2.21391715917256;-4.55929623734289;0.271142654228479]
Robot Arm 1 togoal: 13.4291910846819

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.865228e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-5.9649780232574;-5.08092360379711;-6.20589947769673]
Robot Arm 1 end: [-4.961530873736;-5.41922113830071;0.983198630931249]
Robot Arm 1 togoal: 21.4916034048218
Mobile Platform 2 Pose: [1.01237282659567;2.52259195376732;-4.21280925486745]
Robot Arm 2 end: [2.00073109553588;-4.65783744700893;0.24101075254872]
Robot Arm 1 togoal: 13.1982809503182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.777880e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.529232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-5.81552674485845;-5.14916183113856;-6.1730115319886]
Robot Arm 1 end: [-4.86965535917052;-5.46824739422288;1.06461877767681]
Robot Arm 1 togoal: 21.4611652006474
Mobile Platform 2 Pose: [0.874894547572701;2.39692761186632;-4.26584670241748]
Robot Arm 2 end: [1.88098573311024;-4.74180116762925;0.287847394695049]
Robot Arm 1 togoal: 13.0509441152387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.901623e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.710209e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-5.6858569222057;-5.17737649149485;-6.09311341786351]
Robot Arm 1 end: [-4.85920043956832;-5.5049605564048;1.19049317318583]
Robot Arm 1 togoal: 21.4781022048389
Mobile Platform 2 Pose: [0.644480496576879;2.39360377606798;-4.28845483505264]
Robot Arm 2 end: [1.82166695347651;-4.7906322833609;0.399952889437393]
Robot Arm 1 togoal: 12.9671148753368

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.892997e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.633384e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-5.43501759428033;-5.23066832150406;-5.95612339113662]
Robot Arm 1 end: [-4.92955351085061;-5.55396812754384;1.33039444341106]
Robot Arm 1 togoal: 21.5604689889669
Mobile Platform 2 Pose: [0.780147470146818;2.15348293446191;-4.36496464847631]
Robot Arm 2 end: [1.57307127223508;-4.74115761561949;0.232320458342611]
Robot Arm 1 togoal: 12.7770143952755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.643244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.837097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-5.7076452568995;-5.03447233904447;-5.87191963718533]
Robot Arm 1 end: [-4.85877140457309;-5.19924770873324;1.15525716848865]
Robot Arm 1 togoal: 21.2587390591162
Mobile Platform 2 Pose: [0.571546266982296;2.05769518927544;-4.4052560784207]
Robot Arm 2 end: [1.49723055050771;-4.90950279347663;0.340808589045163]
Robot Arm 1 togoal: 12.629027037252

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.780957e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-5.49822029070463;-5.11546891924025;-5.80506126075058]
Robot Arm 1 end: [-4.92370130962215;-5.29379907415224;1.3035852749378]
Robot Arm 1 togoal: 21.3697080013282
Mobile Platform 2 Pose: [0.758906929518801;1.83586373686586;-4.46629923344581]
Robot Arm 2 end: [1.23368929340566;-4.86753115103393;0.253098840594917]
Robot Arm 1 togoal: 12.4155380461905

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.040154e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-5.84331529779853;-4.86391877256719;-5.65787993178628]
Robot Arm 1 end: [-4.75543076533644;-5.06216370375994;1.26977190190863]
Robot Arm 1 togoal: 21.086835420868
Mobile Platform 2 Pose: [0.579935110449988;1.69091372209213;-4.53878460943364]
Robot Arm 2 end: [1.10190367146153;-4.91270614694079;0.239631431520877]
Robot Arm 1 togoal: 12.2790361708919

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.977663e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-5.57626428449496;-4.98284086408013;-5.59163205166755]
Robot Arm 1 end: [-4.87145487301729;-5.12973320798908;1.35606694928475]
Robot Arm 1 togoal: 21.2154707677283
Mobile Platform 2 Pose: [0.32770134906497;1.57196488523392;-4.58282056653234]
Robot Arm 2 end: [1.00747802044186;-5.04874926074463;0.333956937719337]
Robot Arm 1 togoal: 12.1280059154502

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-5.90141713901478;-4.78615122544889;-5.53939797985285]
Robot Arm 1 end: [-4.75272979547982;-4.79145667408726;1.09125741520187]
Robot Arm 1 togoal: 20.8953321497656
Mobile Platform 2 Pose: [0.307903140262191;1.33759820282541;-4.669839289674]
Robot Arm 2 end: [0.758856226765702;-5.03002662339621;0.251257324901928]
Robot Arm 1 togoal: 11.9191387748164

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.098002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-6.15714451855459;-4.61076747296156;-5.45762610680789]
Robot Arm 1 end: [-4.45165102568828;-4.68533786157269;1.01256785985628]
Robot Arm 1 togoal: 20.6098973485544
Mobile Platform 2 Pose: [0.227962040552488;1.09258182862722;-4.7195079555995]
Robot Arm 2 end: [0.548882226786175;-5.16514712594061;0.25564083264008]
Robot Arm 1 togoal: 11.67286821526

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.968570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-6.44903556690384;-4.401224861843;-5.34787883945885]
Robot Arm 1 end: [-4.07834253697805;-4.60325335349305;0.941136526579397]
Robot Arm 1 togoal: 20.2926336078538
Mobile Platform 2 Pose: [-0.0482078542663627;1.00442863140308;-4.7720459245386]
Robot Arm 2 end: [0.360439501775629;-5.16303352500279;0.4203710302062]
Robot Arm 1 togoal: 11.4867896071683

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.714075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-6.68751505969502;-4.19411594646096;-5.20785777967651]
Robot Arm 1 end: [-3.76129963276098;-4.52783976008119;0.954053367978091]
Robot Arm 1 togoal: 20.0188142546152
Mobile Platform 2 Pose: [-0.0320772474255973;0.758955165865599;-4.81940389880676]
Robot Arm 2 end: [0.217530510821495;-5.24751531526157;0.233235207368473]
Robot Arm 1 togoal: 11.3420623600553

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.257378e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-6.35859500108116;-4.27055771282909;-4.99527260089166]
Robot Arm 1 end: [-3.99254548698645;-4.26012757234071;1.02449833587971]
Robot Arm 1 togoal: 19.9847212011399
Mobile Platform 2 Pose: [-0.464246508164431;0.796171254534033;-4.85384595906049]
Robot Arm 2 end: [0.142768071696553;-5.23411450278635;0.429500211611923]
Robot Arm 1 togoal: 11.2596971769595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.794680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-6.18708370865276;-4.34235725794971;-4.94808641009193]
Robot Arm 1 end: [-4.13977069380554;-4.27515488219964;1.14448270712758]
Robot Arm 1 togoal: 20.0961421037975
Mobile Platform 2 Pose: [-0.436365030716378;0.552552993035559;-4.90997209182464]
Robot Arm 2 end: [-0.00010529953855265;-5.32990268231011;0.240161952736928]
Robot Arm 1 togoal: 11.1107267575226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.846311e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-5.91558244803533;-4.47384331750719;-4.90159791372351]
Robot Arm 1 end: [-4.37348529566845;-4.26651125328847;1.1802285772798]
Robot Arm 1 togoal: 20.2545438864356
Mobile Platform 2 Pose: [-0.879159255475994;0.623546211631168;-4.93435343458754]
Robot Arm 2 end: [-0.0367666651744725;-5.30659845461057;0.437304151222354]
Robot Arm 1 togoal: 11.0665456779928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.995193e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-6.04342735521579;-4.3872162218525;-4.85916044061236]
Robot Arm 1 end: [-4.16388370892719;-4.19212803910665;1.10692633593836]
Robot Arm 1 togoal: 20.0550132634631
Mobile Platform 2 Pose: [-0.716707618585408;0.379528890203726;-4.98871837806013]
Robot Arm 2 end: [-0.158089063295672;-5.39418924589795;0.254459075246017]
Robot Arm 1 togoal: 10.9398733769043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.275076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-6.22923178822151;-4.258037789018;-4.80938603416768]
Robot Arm 1 end: [-3.93915213484135;-4.13047941962229;1.05361316591044]
Robot Arm 1 togoal: 19.8541900192202
Mobile Platform 2 Pose: [-1.00293392649069;0.270497863621208;-5.01419193268524]
Robot Arm 2 end: [-0.363451748804126;-5.37283958134937;0.407796402344224]
Robot Arm 1 togoal: 10.7476926820312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.767924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-6.36802037650495;-4.17219546662843;-4.79252572808347]
Robot Arm 1 end: [-3.74221824113689;-4.15705755196679;0.99709221596675]
Robot Arm 1 togoal: 19.7369024946395
Mobile Platform 2 Pose: [-1.02144594141574;0.0205356625680551;-5.00984474438688]
Robot Arm 2 end: [-0.49154013586962;-5.39294556691613;0.233639301168812]
Robot Arm 1 togoal: 10.6439211339652

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.259011e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-6.53153874942606;-4.0747656477554;-4.77170568279561]
Robot Arm 1 end: [-3.58479745103081;-4.27574059552606;1.08299125655536]
Robot Arm 1 togoal: 19.7112998809215
Mobile Platform 2 Pose: [-1.12935872191416;-0.228776867686583;-5.02837208251315]
Robot Arm 2 end: [-0.781771247945944;-5.35212814198535;0.223138679403578]
Robot Arm 1 togoal: 10.4049458499898

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.231609e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-6.76673337753509;-3.93228313990146;-4.704727976241]
Robot Arm 1 end: [-3.46497134851771;-4.38282379675944;1.20730863771403]
Robot Arm 1 togoal: 19.7045547033118
Mobile Platform 2 Pose: [-1.1820613753605;-0.476585689401585;-5.06139916300018]
Robot Arm 2 end: [-0.948558021948889;-5.45535012153616;0.159045251137193]
Robot Arm 1 togoal: 10.2194601196833
Step 140:
Mobile Platform 1 Pose: [-6.62634943046325;-4.01982230225326;-4.59915337004404]
Robot Arm 1 end: [-3.53573653654132;-4.07866109789511;1.02363070952827]
Robot Arm 1 togoal: 19.5364336076764
Mobile Platform 2 Pose: [-1.060153956324;-0.716681246380522;-5.13107070323702]
Robot Arm 2 end: [-1.27631876824466;-5.49846297831594;0.182246918118989]
Robot Arm 1 togoal: 9.90750773934184
Step 141:
Mobile Platform 1 Pose: [-6.85442797360757;-3.89530863633355;-4.5817044361955]
Robot Arm 1 end: [-3.42895502178502;-4.24580480837777;1.17957061789864]
Robot Arm 1 togoal: 19.580509738985
Mobile Platform 2 Pose: [-0.788596838095259;-0.963242844719942;-5.17239113678886]
Robot Arm 2 end: [-1.59080721655332;-5.37283892524737;0.139874002908191]
Robot Arm 1 togoal: 9.69704346183518
Step 142:
Mobile Platform 1 Pose: [-7.12043574114611;-3.74854507818352;-4.55951027523487]
Robot Arm 1 end: [-3.3360127922636;-4.35882031790938;1.25265542501945]
Robot Arm 1 togoal: 19.5983919475131
Mobile Platform 2 Pose: [-1.16762999625054;-1.00825120156589;-5.18929985931467]
Robot Arm 2 end: [-1.62459885299771;-5.56189249576706;0.266817568382234]
Robot Arm 1 togoal: 9.56122984579467
Step 143:
Mobile Platform 1 Pose: [-7.42887967543602;-3.55011356196986;-4.64558973818682]
Robot Arm 1 end: [-3.04471598880897;-4.57695272697606;1.20256891707948]
Robot Arm 1 togoal: 19.5640886418906
Mobile Platform 2 Pose: [-1.34218807765012;-1.18351452427084;-5.24016000171017]
Robot Arm 2 end: [-1.78462499667952;-5.71427854292636;0.361574230460735]
Robot Arm 1 togoal: 9.33831156943674
Step 144:
Mobile Platform 1 Pose: [-7.30057489265191;-3.62499149077696;-4.64298713848625]
Robot Arm 1 end: [-3.01091408552354;-4.48412993960637;1.06354987399638]
Robot Arm 1 togoal: 19.4751936079223
Mobile Platform 2 Pose: [-1.33873490365027;-1.41417956430467;-5.32552892147984]
Robot Arm 2 end: [-2.03952258771831;-5.69482591136119;0.268437151031024]
Robot Arm 1 togoal: 9.13633615053383
Step 145:
Mobile Platform 1 Pose: [-7.07025292849768;-3.75763712709537;-4.62235714542353]
Robot Arm 1 end: [-3.12270119864864;-4.32041718702283;0.935389473467498]
Robot Arm 1 togoal: 19.4325133435739
Mobile Platform 2 Pose: [-1.73026650178312;-1.4251070405704;-5.36031491945088]
Robot Arm 2 end: [-2.0170494754135;-5.80909708847155;0.37396908443578]
Robot Arm 1 togoal: 9.08883084962602
Step 146:
Mobile Platform 1 Pose: [-6.65571007486871;-3.98207565108491;-4.51223290180949]
Robot Arm 1 end: [-3.38812720123399;-4.03784811625424;0.724806506312763]
Robot Arm 1 togoal: 19.4148668234123
Mobile Platform 2 Pose: [-1.70993579607573;-1.65783135379364;-5.45163520252065]
Robot Arm 2 end: [-2.26948459711751;-5.80937686295506;0.273180625597447]
Robot Arm 1 togoal: 8.88139875594347
Step 147:
Mobile Platform 1 Pose: [-6.81482067650157;-3.8920369058696;-4.50978368614136]
Robot Arm 1 end: [-3.2131962538836;-4.16363908340273;0.658340392578361]
Robot Arm 1 togoal: 19.3892085688615
Mobile Platform 2 Pose: [-1.81754857738576;-1.88678535804131;-5.50344418146239]
Robot Arm 2 end: [-2.48642195557482;-5.96266689159223;0.335486268613947]
Robot Arm 1 togoal: 8.61158269223599
Step 148:
Mobile Platform 1 Pose: [-6.98339597493733;-3.79513480147659;-4.50715099484684]
Robot Arm 1 end: [-3.06382069571361;-4.29657457396256;0.593369005964988]
Robot Arm 1 togoal: 19.3885521630606
Mobile Platform 2 Pose: [-2.00616300334506;-2.02215807260071;-5.57365560823253]
Robot Arm 2 end: [-2.66694854938141;-5.93750845113555;0.342386371417624]
Robot Arm 1 togoal: 8.46561345960791
Step 149:
Mobile Platform 1 Pose: [-7.14321044240207;-3.70115168831157;-4.50522591651231]
Robot Arm 1 end: [-2.9497698835472;-4.43701967719883;0.529442416177762]
Robot Arm 1 togoal: 19.4192498217281
Mobile Platform 2 Pose: [-2.22101831758488;-2.17569660047448;-5.61415527168871]
Robot Arm 2 end: [-2.83283896911159;-6.03194549621216;0.448055715291685]
Robot Arm 1 togoal: 8.26225533530673
Step 150:
Mobile Platform 1 Pose: [-7.2979835525518;-3.60657374511668;-4.50398744447661]
Robot Arm 1 end: [-2.87220202537749;-4.57824039292654;0.448132280581866]
Robot Arm 1 togoal: 19.4774157187101
Mobile Platform 2 Pose: [-2.38283211308376;-2.33625193592434;-5.68164579757761]
Robot Arm 2 end: [-3.02579586103817;-6.00444150780304;0.446403127075753]
Robot Arm 1 togoal: 8.1091781011258
Step 151:
Mobile Platform 1 Pose: [-7.46583604824248;-3.49526424256146;-4.50297130839059]
Robot Arm 1 end: [-2.83275444164163;-4.70515620547703;0.320336142382961]
Robot Arm 1 togoal: 19.5540992962074
Mobile Platform 2 Pose: [-2.33267207807098;-2.58204012483647;-5.72734225616767]
Robot Arm 2 end: [-3.17792824458799;-6.10345037705325;0.256896445999098]
Robot Arm 1 togoal: 7.95749456772133
Step 152:
Mobile Platform 1 Pose: [-7.66433569201046;-3.33035529243294;-4.50211722525375]
Robot Arm 1 end: [-2.78305319755946;-4.81091149037018;0.207646751027434]
Robot Arm 1 togoal: 19.6085299036874
Mobile Platform 2 Pose: [-2.77970427135425;-2.55513293696807;-5.7554402985182]
Robot Arm 2 end: [-3.25013599145808;-6.06992550563151;0.452298336178765]
Robot Arm 1 togoal: 7.88341759082631
Step 153:
Mobile Platform 1 Pose: [-7.84322942601661;-3.16699056065027;-4.51633865233963]
Robot Arm 1 end: [-2.71040832160396;-4.93986539775599;0.190977352687444]
Robot Arm 1 togoal: 19.6601886574705
Mobile Platform 2 Pose: [-2.72493854239825;-2.80130039639618;-5.79904743313036]
Robot Arm 2 end: [-3.40252846932877;-6.16656129036989;0.262713212720465]
Robot Arm 1 togoal: 7.73338015214242
Step 154:
Mobile Platform 1 Pose: [-8.00374863366912;-3.05311444475518;-4.53334271707912]
Robot Arm 1 end: [-2.58715845980107;-4.96404341817331;0.182031565804811]
Robot Arm 1 togoal: 19.5997956617126
Mobile Platform 2 Pose: [-3.18883092010971;-2.76297450743148;-5.82399489480507]
Robot Arm 2 end: [-3.46056774204495;-6.13001305849002;0.459404144561715]
Robot Arm 1 togoal: 7.67254579289851
Step 155:
Mobile Platform 1 Pose: [-8.15037775418756;-2.95373178204674;-4.54641883907772]
Robot Arm 1 end: [-2.4232072956865;-4.91737175402922;0.17739628147538]
Robot Arm 1 togoal: 19.459427805358
Mobile Platform 2 Pose: [-3.02377758572103;-3.01007780888025;-5.86194168104453]
Robot Arm 2 end: [-3.58852319369677;-6.21640516425668;0.278860971702691]
Robot Arm 1 togoal: 7.54755152910181
Step 156:
Mobile Platform 1 Pose: [-8.30507544754385;-2.84609237208542;-4.55678889492316]
Robot Arm 1 end: [-2.24205154675412;-4.82409730230684;0.175946650542283]
Robot Arm 1 togoal: 19.2725414882037
Mobile Platform 2 Pose: [-3.12941393810546;-3.25826936237893;-5.89195755638669]
Robot Arm 2 end: [-3.83432624347729;-6.16625332906303;0.219021181373234]
Robot Arm 1 togoal: 7.37619788596712
Step 157:
Mobile Platform 1 Pose: [-8.47129075714681;-2.71816650451507;-4.56407685230678]
Robot Arm 1 end: [-2.06468206392304;-4.7146832879699;0.172069219725718]
Robot Arm 1 togoal: 19.0761125971404
Mobile Platform 2 Pose: [-3.08063495151795;-3.50535973792895;-5.92998843834445]
Robot Arm 2 end: [-4.00693374344322;-6.28468949687704;0.158023017825478]
Robot Arm 1 togoal: 7.18178818568418
Step 158:
Mobile Platform 1 Pose: [-8.61472454861527;-2.56874795030895;-4.56450087675956]
Robot Arm 1 end: [-1.89584652535814;-4.5967750941158;0.160075071130723]
Robot Arm 1 togoal: 18.879330612209
Mobile Platform 2 Pose: [-2.81948039052967;-3.75036134692103;-5.97972988741111]
Robot Arm 2 end: [-4.34447184856484;-6.28821446550159;0.182222875932349]
Robot Arm 1 togoal: 6.89598975591714
Step 159:
Mobile Platform 1 Pose: [-8.64983161246607;-2.44952714789898;-4.54975417149182]
Robot Arm 1 end: [-1.77166973153319;-4.62832171521792;0.164953177287415]
Robot Arm 1 togoal: 18.8254845211909
Mobile Platform 2 Pose: [-2.79919220316609;-3.99012836493911;-6.05052379301547]
Robot Arm 2 end: [-4.63765017625016;-6.18564638454151;0.143263608500522]
Robot Arm 1 togoal: 6.72326161178436
Step 160:
Mobile Platform 1 Pose: [-8.72768139952988;-2.30269756057527;-4.53718622542108]
Robot Arm 1 end: [-1.62200126648241;-4.57272864191243;0.15426275295227]
Robot Arm 1 togoal: 18.6896576804232
Mobile Platform 2 Pose: [-3.25020178843744;-3.98287408742179;-6.06377424812163]
Robot Arm 2 end: [-4.65154850602443;-6.38591387914927;0.312907201562044]
Robot Arm 1 togoal: 6.56711809563168
Step 161:
Mobile Platform 1 Pose: [-8.76242224086431;-2.16872223358471;-4.51547256930249]
Robot Arm 1 end: [-1.4742755502386;-4.59713073896509;0.175144971358889]
Robot Arm 1 togoal: 18.6157608217167
Mobile Platform 2 Pose: [-3.32879919240021;-4.22354719279717;-6.13142375922948]
Robot Arm 2 end: [-4.85985445958458;-6.52838796713428;0.291175343493326]
Robot Arm 1 togoal: 6.32342114437607
Step 162:
Mobile Platform 1 Pose: [-8.84841719854036;-2.00898350334878;-4.49732954465901]
Robot Arm 1 end: [-1.33899803077187;-4.51105872154672;0.12796918465093]
Robot Arm 1 togoal: 18.4684660005789
Mobile Platform 2 Pose: [-3.69389660097133;-4.23662841897135;-6.18740409899882]
Robot Arm 2 end: [-4.9002149934472;-6.6259558971483;0.480910798132298]
Robot Arm 1 togoal: 6.2027224884635
Step 163:
Mobile Platform 1 Pose: [-8.87616729209108;-1.87687019637401;-4.46774637531011]
Robot Arm 1 end: [-1.18618962181761;-4.58317874941757;0.176468855530475]
Robot Arm 1 togoal: 18.4284482442547
Mobile Platform 2 Pose: [-3.78419182337375;-4.43389070131552;-6.27292299760582]
Robot Arm 2 end: [-5.1570425195823;-6.63824295357529;0.474046066226837]
Robot Arm 1 togoal: 5.98763393468263
Step 164:
Mobile Platform 1 Pose: [-9.047839429309;-1.70655259971131;-4.45921208442636]
Robot Arm 1 end: [-1.02160291459203;-4.46931288891083;0.143713956015116]
Robot Arm 1 togoal: 18.2409885458848
Mobile Platform 2 Pose: [-4.13163204914332;-4.45435615202413;-6.33140769928663]
Robot Arm 2 end: [-5.21805998106457;-6.60419378858605;0.658327387396982]
Robot Arm 1 togoal: 5.92812400399225
Step 165:
Mobile Platform 1 Pose: [-9.09300073563555;-1.58122634839351;-4.43739929048271]
Robot Arm 1 end: [-0.862416902908385;-4.47240601050448;0.193872183627179]
Robot Arm 1 togoal: 18.143977098378
Mobile Platform 2 Pose: [-3.90520637639144;-4.69521762738439;-6.39838343897556]
Robot Arm 2 end: [-5.50400640712627;-6.46785714575233;0.482182106731872]
Robot Arm 1 togoal: 5.81112159099674
Step 166:
Mobile Platform 1 Pose: [-9.14870277824688;-1.45458059954615;-4.41778836975127]
Robot Arm 1 end: [-0.715790183151017;-4.41035753422323;0.215392562521645]
Robot Arm 1 togoal: 18.0053096139516
Mobile Platform 2 Pose: [-4.02671183356726;-4.94387666536511;-6.42424234243146]
Robot Arm 2 end: [-5.71451744228826;-6.39371489863925;0.370952360519127]
Robot Arm 1 togoal: 5.71780224892603
Step 167:
Mobile Platform 1 Pose: [-9.11902249860335;-1.36570888406503;-4.38819117926329]
Robot Arm 1 end: [-0.619797709689216;-4.48409189583159;0.250283233694651]
Robot Arm 1 togoal: 18.0051032360768
Mobile Platform 2 Pose: [-4.08476018488742;-5.19265951274692;-6.44888164200915]
Robot Arm 2 end: [-5.90625551331931;-6.48627062380739;0.213605537570383]
Robot Arm 1 togoal: 5.55106461246301
Step 168:
Mobile Platform 1 Pose: [-9.16286241393786;-1.24135679140546;-4.37089863180724]
Robot Arm 1 end: [-0.47278386662244;-4.44650026135605;0.281914892830441]
Robot Arm 1 togoal: 17.8861931002983
Mobile Platform 2 Pose: [-4.60870180825678;-5.12705960457486;-6.45138498224758]
Robot Arm 2 end: [-5.90117291920931;-6.40435294798808;0.383494830511479]
Robot Arm 1 togoal: 5.5698275710724
Step 169:
Mobile Platform 1 Pose: [-9.10163044953059;-1.19387056952256;-4.35575823284448]
Robot Arm 1 end: [-0.419984286485164;-4.54989547152798;0.371804133900863]
Robot Arm 1 togoal: 17.9331029635601
Mobile Platform 2 Pose: [-4.30746408030731;-5.37571423001907;-6.4772863466922]
Robot Arm 2 end: [-6.08707774537389;-6.51109230854488;0.241823852883495]
Robot Arm 1 togoal: 5.39626992149158
Step 170:
Mobile Platform 1 Pose: [-9.12167635664381;-1.09218317058162;-4.35031827365889]
Robot Arm 1 end: [-0.330560632351584;-4.59571799731807;0.385012278666947]
Robot Arm 1 togoal: 17.9177547404508
Mobile Platform 2 Pose: [-4.2163654078123;-5.62255906383758;-6.51687966847653]
Robot Arm 2 end: [-6.27026217944162;-6.7325157103694;0.225423094223377]
Robot Arm 1 togoal: 5.12502851815168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.493560e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-9.12481928871772;-1.01403646818894;-4.35093222532677]
Robot Arm 1 end: [-0.289447064125596;-4.69087716894226;0.40289752313398]
Robot Arm 1 togoal: 17.9706634832208
Mobile Platform 2 Pose: [-4.27508322250213;-5.91100982378555;-6.55262339178313]
Robot Arm 2 end: [-6.52511389958457;-6.83404190486247;0.184695577988576]
Robot Arm 1 togoal: 4.88710891829868

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.285150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-9.19801697391488;-0.905115881626079;-4.35852079441525]
Robot Arm 1 end: [-0.187929100016902;-4.65816762921137;0.326957555680319]
Robot Arm 1 togoal: 17.8908220870889
Mobile Platform 2 Pose: [-4.85160066426821;-5.77667495158825;-6.56937723070633]
Robot Arm 2 end: [-6.57657859863056;-6.66963965416425;0.566861969594987]
Robot Arm 1 togoal: 4.87047158964455

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.990496e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-9.19643822411052;-0.837353473096056;-4.36272330236952]
Robot Arm 1 end: [-0.171864860799649;-4.76681211681952;0.344628636011729]
Robot Arm 1 togoal: 17.9696806990506
Mobile Platform 2 Pose: [-4.59456717395014;-5.97198049153947;-6.58608266636193]
Robot Arm 2 end: [-6.68241475833099;-6.79283506464033;0.387453244913431]
Robot Arm 1 togoal: 4.75154785352183

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.601950e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-9.25956993257499;-0.740299465953319;-4.37425191198495]
Robot Arm 1 end: [-0.10121923935599;-4.76703611841603;0.273453402161345]
Robot Arm 1 togoal: 17.9347806888715
Mobile Platform 2 Pose: [-4.29914640632483;-6.34451903085906;-6.58908167570594]
Robot Arm 2 end: [-6.898873514105;-6.9510693613479;0.255165467071604]
Robot Arm 1 togoal: 4.5293819209231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.211497e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-9.26984915903877;-0.675459903081156;-4.38422045424078]
Robot Arm 1 end: [-0.102905315042427;-4.85804670680279;0.268208165427102]
Robot Arm 1 togoal: 18.0111003308243
Mobile Platform 2 Pose: [-4.42917941331464;-6.59404601586048;-6.60449002909078]
Robot Arm 2 end: [-7.14843548021376;-7.06975815175469;0.337240316386849]
Robot Arm 1 togoal: 4.25664474536691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.471561e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-9.31406264541143;-0.596203749131776;-4.3995847472384]
Robot Arm 1 end: [-0.0835257975792835;-4.89805943682078;0.217386000659824]
Robot Arm 1 togoal: 18.0368810206412
Mobile Platform 2 Pose: [-4.703462701377;-6.76365730814769;-6.66917196157077]
Robot Arm 2 end: [-7.34022730271122;-7.14894242329163;0.429566031533869]
Robot Arm 1 togoal: 4.04896878408204

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.061753e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-9.33741836722621;-0.535775326077849;-4.41520724251836]
Robot Arm 1 end: [-0.110258710556405;-4.96632732034192;0.19250348816478]
Robot Arm 1 togoal: 18.1100300285768
Mobile Platform 2 Pose: [-5.17450182983622;-6.72998881357898;-6.68005968041215]
Robot Arm 2 end: [-7.44092415930493;-7.00554980584895;0.542576028142103]
Robot Arm 1 togoal: 4.05868384977034

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.015978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-9.37190035174297;-0.474068285670945;-4.43407179101044]
Robot Arm 1 end: [-0.143500416631358;-5.01888156310757;0.16027290194558]
Robot Arm 1 togoal: 18.1744045939401
Mobile Platform 2 Pose: [-5.1849003589275;-6.97645233717752;-6.70896231718013]
Robot Arm 2 end: [-7.64192646615938;-6.95533293225822;0.330956022563415]
Robot Arm 1 togoal: 4.03072115292338

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.132481e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-9.40857407951934;-0.418913725955344;-4.45420935898747]
Robot Arm 1 end: [-0.189492752904284;-5.08430816378665;0.162480522481171]
Robot Arm 1 togoal: 18.2539773888423
Mobile Platform 2 Pose: [-5.57149805248987;-7.01075254018485;-6.69720528490882]
Robot Arm 2 end: [-7.71609490544098;-6.81428461588635;0.354743908467863]
Robot Arm 1 togoal: 4.08964035834044

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-9.48056948333398;-0.356315500267033;-4.47529791072401]
Robot Arm 1 end: [-0.21170836299462;-5.12842432592093;0.159565123149037]
Robot Arm 1 togoal: 18.3030520733531
Mobile Platform 2 Pose: [-5.45846748389493;-7.23723195518233;-6.6926085746491]
Robot Arm 2 end: [-7.78993722678617;-6.81843375772976;0.112403198729329]
Robot Arm 1 togoal: 4.12200026249989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993206e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.104916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-9.58046753012283;-0.287665745600379;-4.49608338756275]
Robot Arm 1 end: [-0.201051110650997;-5.16202972873574;0.158668162848753]
Robot Arm 1 togoal: 18.3249703217139
Mobile Platform 2 Pose: [-5.92810639979936;-7.21507421305207;-6.65898388045362]
Robot Arm 2 end: [-7.82672062882603;-6.87280733206729;0.174364681729829]
Robot Arm 1 togoal: 4.03929497412049

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000243e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.350479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-9.6800838229974;-0.228868328904897;-4.5117157881385]
Robot Arm 1 end: [-0.189441701383255;-5.18512454854388;0.15829247860631]
Robot Arm 1 togoal: 18.3376580164846
Mobile Platform 2 Pose: [-6.1012343563493;-7.46698043076244;-6.68355378390142]
Robot Arm 2 end: [-7.96566260197386;-6.97536333607206;0.0720779531990652]
Robot Arm 1 togoal: 3.92254135675976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.583329e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.082599e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-9.77058388905213;-0.179454333712656;-4.5241657432424]
Robot Arm 1 end: [-0.188323693781263;-5.20197000139608;0.158798539685975]
Robot Arm 1 togoal: 18.3509515947019
Mobile Platform 2 Pose: [-6.02075140654546;-7.70195541820937;-6.71398866080801]
Robot Arm 2 end: [-8.10637181996746;-6.86323233605986;-0.0486279069779507]
Robot Arm 1 togoal: 3.98608464889017

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.021324e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.647053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-9.85690051044267;-0.134708405946525;-4.53504818620852]
Robot Arm 1 end: [-0.149356330329467;-5.21607822418437;0.15990534968481]
Robot Arm 1 togoal: 18.3409664165118
Mobile Platform 2 Pose: [-6.21916476041587;-7.84231733242467;-6.69963397270713]
Robot Arm 2 end: [-8.24177071580439;-6.94028219716809;-0.0457484507719009]
Robot Arm 1 togoal: 3.86108067213904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.312701e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-9.93073341780661;-0.0976022996198911;-4.54347075799515]
Robot Arm 1 end: [-0.179452904841386;-5.22378113345574;0.161260390074845]
Robot Arm 1 togoal: 18.363922926435
Mobile Platform 2 Pose: [-6.2159123640702;-7.92085339258408;-6.70093382709885]
Robot Arm 2 end: [-8.4069620428687;-6.74988819557725;-0.0647780407352443]
Robot Arm 1 togoal: 3.95166909907539

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.486551e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-10.0055998879973;-0.0613610985958422;-4.55186572160255]
Robot Arm 1 end: [-0.216480031664344;-5.22404510697347;0.163255926040576]
Robot Arm 1 togoal: 18.3845446673811
Mobile Platform 2 Pose: [-6.669600359399;-7.91145638691431;-6.68330371221322]
Robot Arm 2 end: [-8.48965082488607;-6.90989423427617;0.0735146239103118]
Robot Arm 1 togoal: 3.73163800335999

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.860059e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-10.0843264478238;-0.0245827369382062;-4.56084222820701]
Robot Arm 1 end: [-0.257710271671568;-5.21570203599184;0.165924504151147]
Robot Arm 1 togoal: 18.4003923345426
Mobile Platform 2 Pose: [-6.63302463431773;-8.1955931147452;-6.73719601754405]
Robot Arm 2 end: [-8.7554402999541;-6.65173238648242;0.0149472266863214]
Robot Arm 1 togoal: 3.87659900102073

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.639734e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-10.1702246681952;0.0140247635247834;-4.57112641756132]
Robot Arm 1 end: [-0.299788238082034;-5.19797575769588;0.169305215842762]
Robot Arm 1 togoal: 18.4089973564657
Mobile Platform 2 Pose: [-7.04619398892462;-8.16576952594049;-6.69468057021861]
Robot Arm 2 end: [-8.78991702099107;-6.59279578816586;0.0750443872146061]
Robot Arm 1 togoal: 3.89411466078915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.264961e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-10.2671511126697;0.055521410367071;-4.58368488344089]
Robot Arm 1 end: [-0.338910362447401;-5.17084671388152;0.17331411184029]
Robot Arm 1 togoal: 18.4082567005123
Mobile Platform 2 Pose: [-7.72460236084832;-7.97702524388777;-6.66158982288392]
Robot Arm 2 end: [-8.70888952116948;-6.8169896018744;0.270898425087341]
Robot Arm 1 togoal: 3.65530784075989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.353137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-10.3791320191553;0.100222986284378;-4.59980822098865]
Robot Arm 1 end: [-0.371465966343036;-5.13528760199534;0.177485050523537]
Robot Arm 1 togoal: 18.3969907731018
Mobile Platform 2 Pose: [-7.49874335331036;-8.24528859843642;-6.6928949498919]
Robot Arm 2 end: [-8.89925596487697;-6.44503406300386;0.146139196473492]
Robot Arm 1 togoal: 3.96732434685335

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.076116e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-10.5089075470217;0.14651563708496;-4.62098055910435]
Robot Arm 1 end: [-0.395509783387741;-5.09303866898179;0.18064658607013]
Robot Arm 1 togoal: 18.3756084850032
Mobile Platform 2 Pose: [-8.11976763853952;-8.12994190600963;-6.64455486615964]
Robot Arm 2 end: [-8.86476863762445;-6.79409131786125;0.235854741286335]
Robot Arm 1 togoal: 3.63568412548834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.611377e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-10.6549694786802;0.189159471137292;-4.64819445865974]
Robot Arm 1 end: [-0.413136290087716;-5.04546978293434;0.181249829107158]
Robot Arm 1 togoal: 18.346511900237
Mobile Platform 2 Pose: [-7.98549931392521;-8.26805867423616;-6.66000011929426]
Robot Arm 2 end: [-9.04071850396436;-6.42329665237997;0.431915627380745]
Robot Arm 1 togoal: 3.85993945499099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-10.8086049490604;0.218456357164897;-4.68085554118945]
Robot Arm 1 end: [-0.432584351184931;-4.99146316035773;0.179244112989979]
Robot Arm 1 togoal: 18.3134675251105
Mobile Platform 2 Pose: [-8.22738770529497;-8.42776351862657;-6.64765247014829]
Robot Arm 2 end: [-9.22003659409985;-6.49554165650784;0.478660439235362]
Robot Arm 1 togoal: 3.73845462133088

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003623e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-10.9513204361843;0.219665760124078;-4.71649464358598]
Robot Arm 1 end: [-0.469011432772046;-4.92323471667095;0.176382172978755]
Robot Arm 1 togoal: 18.2787070628398
Mobile Platform 2 Pose: [-8.12779306601191;-8.63617360756633;-6.66414989788235]
Robot Arm 2 end: [-9.39287273870595;-6.38781874722006;0.676447242710079]
Robot Arm 1 togoal: 3.75895281616932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.006563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-11.0427080604668;0.16721944882212;-4.74966830544499]
Robot Arm 1 end: [-0.54389119555473;-4.82839093919176;0.173600823456912]
Robot Arm 1 togoal: 18.2447335943869
Mobile Platform 2 Pose: [-8.6480611761873;-8.5985857265905;-6.63036912920517]
Robot Arm 2 end: [-9.40274097888451;-6.71899185495551;0.572442886187396]
Robot Arm 1 togoal: 3.46720253004012

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.256089e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-11.0175211229604;0.0358973574895699;-4.76711726534965]
Robot Arm 1 end: [-0.659547645396908;-4.74313178686108;0.148606990809292]
Robot Arm 1 togoal: 18.2447075465987
Mobile Platform 2 Pose: [-8.66543897013758;-8.85006033593336;-6.69380540521007]
Robot Arm 2 end: [-9.66199040818191;-6.60202625853257;0.654583348851345]
Robot Arm 1 togoal: 3.5229467559489

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.666029e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-10.8170657028884;-0.169375416472233;-4.7660865131312]
Robot Arm 1 end: [-0.846139035144629;-4.67959388860077;0.133998669920762]
Robot Arm 1 togoal: 18.3044525018798
Mobile Platform 2 Pose: [-9.09431052032469;-8.79782096287082;-6.70400457697652]
Robot Arm 2 end: [-9.58261517953508;-6.87649214846778;0.725227063810024]
Robot Arm 1 togoal: 3.25019475615084

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987162e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.089256e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-10.5782479123142;-0.37441422471701;-4.75711138181863]
Robot Arm 1 end: [-1.04430997773904;-4.64728309990155;0.133746586015134]
Robot Arm 1 togoal: 18.3968518265398
Mobile Platform 2 Pose: [-9.10229402186932;-9.00222848007035;-6.78680628198432]
Robot Arm 2 end: [-9.79662847168404;-6.93968362494659;0.723781648951774]
Robot Arm 1 togoal: 3.16898302191691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.372941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-10.3293312803945;-0.574992039339441;-4.74654795942991]
Robot Arm 1 end: [-1.23060748906931;-4.61590223646156;0.124438177047193]
Robot Arm 1 togoal: 18.4851704802707
Mobile Platform 2 Pose: [-9.27542002844593;-9.08014305285658;-6.84671349635706]
Robot Arm 2 end: [-9.86717091280475;-6.98291288732429;0.765437002808844]
Robot Arm 1 togoal: 3.1130906916766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.198554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-10.0722638620867;-0.773432718280856;-4.73541125759615]
Robot Arm 1 end: [-1.42611500559817;-4.58531576080568;0.130055885706686]
Robot Arm 1 togoal: 18.5801578435881
Mobile Platform 2 Pose: [-9.2593412357076;-9.31506685852739;-6.92569730544457]
Robot Arm 2 end: [-10.0880155343441;-7.06031648107227;0.882638479799888]
Robot Arm 1 togoal: 3.00948356968657

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998593e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-9.82467840646789;-0.9610568192146;-4.72298252955626]
Robot Arm 1 end: [-1.59742681939146;-4.44883912413003;0.165012438798849]
Robot Arm 1 togoal: 18.5770816634977
Mobile Platform 2 Pose: [-9.57699545819867;-9.34867640610838;-6.99861199220609]
Robot Arm 2 end: [-9.93734488572078;-7.29213374679139;1.08674205901838]
Robot Arm 1 togoal: 2.74318159586947

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.744636e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-9.60871530452344;-1.1114793527519;-4.70162474020553]
Robot Arm 1 end: [-1.785958087907;-4.55249403111577;0.167827482326765]
Robot Arm 1 togoal: 18.7753819252272
Mobile Platform 2 Pose: [-9.16240131042469;-9.63304444609139;-7.10784725651303]
Robot Arm 2 end: [-10.2382882181877;-6.96952955120327;1.13483690479156]
Robot Arm 1 togoal: 3.06425428971851

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000546e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-9.44484267940391;-1.26386288827507;-4.69292150338051]
Robot Arm 1 end: [-1.89511505676302;-4.5600606237692;0.116336320508325]
Robot Arm 1 togoal: 18.8537054085914
Mobile Platform 2 Pose: [-9.58515737298651;-9.63919891660282;-7.14811266490394]
Robot Arm 2 end: [-9.92273839965765;-7.07572418236306;1.14629253341769]
Robot Arm 1 togoal: 2.94919719551206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999256e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.103723e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-9.14071900076291;-1.48108328650608;-4.69574826081416]
Robot Arm 1 end: [-1.99710389486006;-4.45218622673909;0.0443119628975209]
Robot Arm 1 togoal: 18.8408279052523
Mobile Platform 2 Pose: [-9.5372018354267;-9.67947656795143;-7.15449551825891]
Robot Arm 2 end: [-9.99299954224822;-7.07408646118967;1.19874471620063]
Robot Arm 1 togoal: 2.94361470150883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-8.7466950888744;-1.73012743082915;-4.67390275749552]
Robot Arm 1 end: [-2.14179075364988;-4.37491085783896;-0.0168966908149433]
Robot Arm 1 togoal: 18.8792550464221
Mobile Platform 2 Pose: [-9.51828482730798;-9.69476104909745;-7.15897629361051]
Robot Arm 2 end: [-10.1498048295873;-6.99535102635348;1.12565474202381]
Robot Arm 1 togoal: 3.03424699304573

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985361e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-8.43084183628488;-1.92186815934181;-4.63994028563692]
Robot Arm 1 end: [-2.21393835466323;-4.34917121653112;-0.0787102174703495]
Robot Arm 1 togoal: 18.911321411068
Mobile Platform 2 Pose: [-9.50508129069307;-9.7080663504041;-7.16106715679135]
Robot Arm 2 end: [-10.3102999963638;-7.04405056327798;0.994416848946807]
Robot Arm 1 togoal: 3.01847858617114

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979590e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-7.88303925038;-2.24309980756753;-4.55082915776191]
Robot Arm 1 end: [-2.31553090793446;-4.35054682221892;-0.175007954186062]
Robot Arm 1 togoal: 18.9864935841679
Mobile Platform 2 Pose: [-9.44095798901989;-9.74583023592201;-7.1668645580026]
Robot Arm 2 end: [-10.4807338804217;-7.0885656354208;0.817905090015082]
Robot Arm 1 togoal: 3.03346296754793

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.454396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-7.89797203430578;-2.20855457491303;-4.24670544844201]
Robot Arm 1 end: [-2.38533636940811;-3.95967815231579;-0.0890356783188316]
Robot Arm 1 togoal: 18.7312942955121
Mobile Platform 2 Pose: [-9.32542160989147;-9.80981523385367;-7.17589125145887]
Robot Arm 2 end: [-10.6132087860134;-7.17943805847327;0.617786044841188]
Robot Arm 1 togoal: 3.02446526348488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000309e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-7.76179618443732;-2.28807689116997;-4.2218771284885]
Robot Arm 1 end: [-2.44626689131318;-3.82263502214171;-0.0890324590433146]
Robot Arm 1 togoal: 18.6699491966183
Mobile Platform 2 Pose: [-9.07836205540389;-9.9373239485122;-7.19447274075824]
Robot Arm 2 end: [-10.7465029756727;-7.28256627043011;0.427566114012687]
Robot Arm 1 togoal: 3.02279844354823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943259e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.998427e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-7.55694527840164;-2.39923334277794;-4.18578179791881]
Robot Arm 1 end: [-2.4523626225081;-3.69256511464279;-0.0441931149017872]
Robot Arm 1 togoal: 18.5741084251198
Mobile Platform 2 Pose: [-8.63654924664669;-10.1407229729065;-7.22356370885151]
Robot Arm 2 end: [-10.9523561181088;-7.39110101657291;0.387395621320942]
Robot Arm 1 togoal: 2.99973248287019

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-7.32533846215528;-2.52000363050566;-4.14348787631569]
Robot Arm 1 end: [-2.39763778381233;-3.60912222005947;0.0151429813966803]
Robot Arm 1 togoal: 18.4709836115894
Mobile Platform 2 Pose: [-8.06260925031966;-10.3651862933145;-7.23301013010402]
Robot Arm 2 end: [-11.2098587884799;-7.46097455592704;0.542829642419021]
Robot Arm 1 togoal: 2.97778873363476

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.246936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.519971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-7.00232666472971;-2.663579506173;-3.97869644132162]
Robot Arm 1 end: [-2.38089315285982;-3.5123393968628;0.0250867578629663]
Robot Arm 1 togoal: 18.3877020768377
Mobile Platform 2 Pose: [-7.3310466007194;-10.6183225349949;-7.21468727287205]
Robot Arm 2 end: [-11.4440813361156;-7.30915705816166;0.785526072200827]
Robot Arm 1 togoal: 3.14116674842818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.322972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.099779e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-7.22494740466307;-2.53160067968336;-3.91255931457464]
Robot Arm 1 end: [-2.46878663702648;-3.32241426207981;0.07704673863078]
Robot Arm 1 togoal: 18.3041624498685
Mobile Platform 2 Pose: [-8.04635274964128;-10.2000854071188;-7.17416259728131]
Robot Arm 2 end: [-10.9441414596945;-7.3848186139273;0.992228591778101]
Robot Arm 1 togoal: 2.83022542920667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001199e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.690935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-6.99329619980733;-2.62318202717879;-3.67630464601446]
Robot Arm 1 end: [-2.47013242703969;-3.10135230630479;0.057573436879286]
Robot Arm 1 togoal: 18.1463840008822
Mobile Platform 2 Pose: [-7.46433544463472;-10.2004538085362;-7.06686900824813]
Robot Arm 2 end: [-10.9023029818805;-7.21957920446521;1.10961234169301]
Robot Arm 1 togoal: 2.95197057518535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.787855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-7.27427416350835;-2.45738073320549;-3.59470204893599]
Robot Arm 1 end: [-2.45216585290187;-2.91350792516001;0.188710153226958]
Robot Arm 1 togoal: 17.9886107745195
Mobile Platform 2 Pose: [-8.12440414572373;-9.86918068017666;-7.01286501349542]
Robot Arm 2 end: [-10.1290297843579;-7.03778099801691;1.42941188125654]
Robot Arm 1 togoal: 2.96644205816599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.106214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-7.20535544394239;-2.45782582967304;-3.32070208188121]
Robot Arm 1 end: [-2.5252475984065;-2.66798008828941;0.127408568130989]
Robot Arm 1 togoal: 17.8690135149555
Mobile Platform 2 Pose: [-8.86502383403682;-9.63023832101468;-6.93928799757463]
Robot Arm 2 end: [-9.75628017783071;-6.95229096924441;1.4839588415434]
Robot Arm 1 togoal: 3.0576627896626

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.107179e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.436185e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-6.88548209956702;-2.61063309551871;-3.12913036789502]
Robot Arm 1 end: [-2.64414767523032;-2.48466348024357;0.0893444109005639]
Robot Arm 1 togoal: 17.8266915150782
Mobile Platform 2 Pose: [-8.92744485646151;-9.87712994234412;-6.81638810167011]
Robot Arm 2 end: [-10.3103861935838;-6.96331956516423;1.38342484288042]
Robot Arm 1 togoal: 3.0556005263022

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.224840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.647053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-6.63518135694301;-2.72607147967576;-2.97743201570165]
Robot Arm 1 end: [-2.71865060856891;-2.29393068350881;0.12079919350661]
Robot Arm 1 togoal: 17.7444460959556
Mobile Platform 2 Pose: [-9.21427843197021;-10.0188178395149;-6.71673396774369]
Robot Arm 2 end: [-10.010325149065;-7.23220273011868;1.29332743328471]
Robot Arm 1 togoal: 2.77716458523805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.193718e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.433290e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-6.41303780301703;-2.83528600243217;-2.93985420532771]
Robot Arm 1 end: [-2.78306175747774;-2.25953225122655;0.124697711530776]
Robot Arm 1 togoal: 17.7666107908948
Mobile Platform 2 Pose: [-9.44768196967003;-10.0819610215421;-6.68417299021611]
Robot Arm 2 end: [-10.3147767311316;-7.21577019565968;1.22222568736859]
Robot Arm 1 togoal: 2.81785132392169

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.466807e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.340772e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-6.16963415811359;-2.95061069564678;-2.88518021600252]
Robot Arm 1 end: [-2.85940232827978;-2.17368246372768;0.164941532515947]
Robot Arm 1 togoal: 17.7595514452216
Mobile Platform 2 Pose: [-10.0054034647104;-9.92027439918345;-6.64312242698566]
Robot Arm 2 end: [-10.238109957009;-7.24333715061967;1.14666763969619]
Robot Arm 1 togoal: 2.7921337956393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.551216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.953325e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-5.94522412249606;-3.05681878886056;-2.8469298033909]
Robot Arm 1 end: [-2.92108598068368;-2.1154119950541;0.212188947240085]
Robot Arm 1 togoal: 17.7609306372709
Mobile Platform 2 Pose: [-10.6685763345693;-9.67104844782068;-6.62346430446573]
Robot Arm 2 end: [-9.85475846454002;-7.3074926151558;1.08444474026109]
Robot Arm 1 togoal: 2.73153283265441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.724606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.665625e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-5.78795600613966;-3.12865830544919;-2.81526013542686]
Robot Arm 1 end: [-2.95320524549081;-2.03388501342786;0.368990920848673]
Robot Arm 1 togoal: 17.7179863293395
Mobile Platform 2 Pose: [-11.4201786075611;-9.39017286436586;-6.58185373166705]
Robot Arm 2 end: [-9.22680477763488;-7.32603348912664;0.914377765739593]
Robot Arm 1 togoal: 2.84884507972259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.830917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.312010e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-5.61506904940688;-3.20537073656709;-2.78267949780679]
Robot Arm 1 end: [-2.99448953695684;-1.99193820392211;0.551181041744527]
Robot Arm 1 togoal: 17.7088647017407
Mobile Platform 2 Pose: [-11.7876512594194;-9.24823719564029;-6.58332793230826]
Robot Arm 2 end: [-9.23857344481415;-7.38120209912726;0.865837687189714]
Robot Arm 1 togoal: 2.80483698290731

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.655725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-5.37444668717978;-3.2965442786392;-2.71584440170765]
Robot Arm 1 end: [-3.03088563415401;-1.9765690203287;0.756369787784849]
Robot Arm 1 togoal: 17.7151586295616
Mobile Platform 2 Pose: [-12.1647823106729;-9.09496659418007;-6.59588479847789]
Robot Arm 2 end: [-9.2402555394689;-7.39286732831701;0.737911295298532]
Robot Arm 1 togoal: 2.82623076419736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.349062e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-5.27490651443238;-3.23854673092561;-2.54573705408697]
Robot Arm 1 end: [-3.02048425788913;-1.84265820375884;0.890205214588321]
Robot Arm 1 togoal: 17.6119126077294
Mobile Platform 2 Pose: [-12.7684286333689;-8.87680583681967;-6.63798930243548]
Robot Arm 2 end: [-8.83909698833195;-7.41059012180784;0.480996975755765]
Robot Arm 1 togoal: 3.02230799393713

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.018378e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-5.37132496759028;-3.09225960185225;-2.39949645644969]
Robot Arm 1 end: [-2.95537363156434;-1.64773759503531;0.847272264990496]
Robot Arm 1 togoal: 17.4346036971637
Mobile Platform 2 Pose: [-12.9777658680362;-8.78906686772905;-6.64456189739687]
Robot Arm 2 end: [-8.96461942930008;-7.35951509011632;0.354928354825629]
Robot Arm 1 togoal: 3.06657733755175

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.247046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-5.23324808968001;-3.09871762721735;-2.2893343183617]
Robot Arm 1 end: [-2.98143393138551;-1.56168793254926;0.873113884811316]
Robot Arm 1 togoal: 17.3956894425842
Mobile Platform 2 Pose: [-13.8765253431732;-8.40259742259981;-6.6095066307756]
Robot Arm 2 end: [-8.46783379957869;-7.31422402884532;0.367525214170266]
Robot Arm 1 togoal: 3.30021664659918

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.148003e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.318642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-5.08216366335187;-3.12866948005789;-2.18781917412036]
Robot Arm 1 end: [-2.96574093342602;-1.50566453456967;0.956210322210582]
Robot Arm 1 togoal: 17.343867553545
Mobile Platform 2 Pose: [-14.7834940297974;-8.09364492261576;-6.58610706442318]
Robot Arm 2 end: [-8.00980648941504;-7.21187765892024;0.246102573134225]
Robot Arm 1 togoal: 3.65511420443077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.881420e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.084646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-5.00571287492978;-3.09069242879306;-2.04894706590219]
Robot Arm 1 end: [-2.960699799906;-1.35253732758984;0.915741631842688]
Robot Arm 1 togoal: 17.2402488604379
Mobile Platform 2 Pose: [-15.0570295662496;-7.98910455344498;-6.5864187177277]
Robot Arm 2 end: [-8.0351940597067;-7.24638371259455;0.246355854346721]
Robot Arm 1 togoal: 3.61491116617332

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.485281e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-4.91007487741454;-3.09040408641907;-1.94209681731264]
Robot Arm 1 end: [-2.93235822097641;-1.27055433584547;0.971290195090267]
Robot Arm 1 togoal: 17.1631426336355
Mobile Platform 2 Pose: [-15.7571846889946;-7.73256853818263;-6.57456195129453]
Robot Arm 2 end: [-7.64857687809652;-7.20823779364674;0.246731389417928]
Robot Arm 1 togoal: 3.86612045944763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.498719e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-4.76098477140611;-3.106721975799;-1.8227070455223]
Robot Arm 1 end: [-2.94203849934131;-1.16157579343929;0.975559205217412]
Robot Arm 1 togoal: 17.0989660614827
Mobile Platform 2 Pose: [-15.9687203122517;-7.65167700647889;-6.5693880045691]
Robot Arm 2 end: [-7.71028143325569;-7.23671218406327;0.305358203739568]
Robot Arm 1 togoal: 3.78897817959055

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.865882e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.012624e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-4.70651249936186;-3.0673023603084;-1.69375160186472]
Robot Arm 1 end: [-2.89191222515615;-1.05724485356179;1.02748819700237]
Robot Arm 1 togoal: 16.9913983671871
Mobile Platform 2 Pose: [-16.1658288086033;-7.5900227327792;-6.60793359413221]
Robot Arm 2 end: [-7.76688934793884;-7.2899649881603;0.357637431157597]
Robot Arm 1 togoal: 3.69925471092889

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.324032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-4.63540832481047;-3.02729913739918;-1.56485019588952]
Robot Arm 1 end: [-2.88182764734097;-0.90920372187485;0.991255234688957]
Robot Arm 1 togoal: 16.888837701202
Mobile Platform 2 Pose: [-16.2619954605403;-7.60375849279829;-6.69329622559487]
Robot Arm 2 end: [-7.91346157190786;-7.375465795252;0.432807450875566]
Robot Arm 1 togoal: 3.52505112413527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.475214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.909599e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-4.74771253562655;-2.91050772405526;-1.46562216728996]
Robot Arm 1 end: [-2.82319402879035;-0.764129087947024;0.961785531779599]
Robot Arm 1 togoal: 16.7515223470423
Mobile Platform 2 Pose: [-16.1062944325791;-7.74336775809546;-6.7996325364892]
Robot Arm 2 end: [-8.30867738936814;-7.36251761231997;0.590287608361033]
Robot Arm 1 togoal: 3.26850287968471

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.009064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.643587e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-4.55754406590124;-2.96094181157792;-1.36504216146362]
Robot Arm 1 end: [-2.86642570548131;-0.736813291661222;1.05868189913363]
Robot Arm 1 togoal: 16.7641822980142
Mobile Platform 2 Pose: [-15.9100645721962;-7.96869955027832;-6.91180454596795]
Robot Arm 2 end: [-8.6475448043324;-7.38361887305114;0.664708824480095]
Robot Arm 1 togoal: 3.06721695271393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.597087e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-4.62931412357291;-2.84888961851137;-1.22451337761757]
Robot Arm 1 end: [-2.7904464212426;-0.59617779738229;1.08575048078312]
Robot Arm 1 togoal: 16.6151721541606
Mobile Platform 2 Pose: [-16.8108637066407;-7.62882386640426;-6.87598795957243]
Robot Arm 2 end: [-7.84304310040717;-7.60749217178735;0.516401222835622]
Robot Arm 1 togoal: 3.37428147578712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.814401e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.107385e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-4.73774394119313;-2.71972611991728;-1.10789106677684]
Robot Arm 1 end: [-2.80378763457531;-0.446178380205088;1.00896834973637]
Robot Arm 1 togoal: 16.5324467926934
Mobile Platform 2 Pose: [-17.4014769499047;-7.45704621033683;-6.92309279336307]
Robot Arm 2 end: [-7.51534144270621;-7.61066182929773;0.318300149965321]
Robot Arm 1 togoal: 3.65088372446749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.806092e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.229538e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-4.77656184011738;-2.66945695006449;-1.02886978465829]
Robot Arm 1 end: [-2.83872999308625;-0.372667202383481;1.02343592902017]
Robot Arm 1 togoal: 16.5128066279329
Mobile Platform 2 Pose: [-17.9027775518685;-7.31334735513208;-7.02188454763584]
Robot Arm 2 end: [-7.26380956382726;-7.72685212223163;0.306984794445485]
Robot Arm 1 togoal: 3.75869289734125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.984212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-4.99034445077108;-2.50687915908138;-0.910078286963159]
Robot Arm 1 end: [-2.82373634867502;-0.217006352563831;0.887829199873349]
Robot Arm 1 togoal: 16.4084227764521
Mobile Platform 2 Pose: [-18.752062661136;-6.98747265831133;-7.01839065873734]
Robot Arm 2 end: [-6.92937027548762;-7.72054731741815;0.366743827687486]
Robot Arm 1 togoal: 3.99461872379707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.498782e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.968030e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-5.16829554389365;-2.37245418588107;-0.788530426076716]
Robot Arm 1 end: [-2.86514515872844;-0.15883412169934;0.810435719262132]
Robot Arm 1 togoal: 16.4078875015773
Mobile Platform 2 Pose: [-19.3308184308652;-6.78808531971343;-7.06408749648331]
Robot Arm 2 end: [-6.85943142173636;-7.7928400534677;0.422088018175665]
Robot Arm 1 togoal: 3.99278518933218

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.723299e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.627619e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-5.32832705350518;-2.27447056344604;-0.754728526658316]
Robot Arm 1 end: [-2.84231035423791;-0.143967979053946;0.591865735281519]
Robot Arm 1 togoal: 16.3917147387487
Mobile Platform 2 Pose: [-19.9084447173635;-6.60947482983357;-7.13844758864846]
Robot Arm 2 end: [-6.81535755718904;-7.81022036381834;0.309140011767364]
Robot Arm 1 togoal: 4.05039343455276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.523828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.053697e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-5.3165168474071;-2.27737694358738;-0.662131096379703]
Robot Arm 1 end: [-2.89562341508558;-0.124529298679228;0.512790518977926]
Robot Arm 1 togoal: 16.4261889414782
Mobile Platform 2 Pose: [-20.7276770925137;-6.33048336052529;-7.22042237715863]
Robot Arm 2 end: [-6.42150187154683;-7.85652569569939;0.242671638639583]
Robot Arm 1 togoal: 4.36282641735816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.140485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.934075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-5.18720085989994;-2.35893488364128;-0.621538578450996]
Robot Arm 1 end: [-2.97624013449892;-0.0951293987778212;0.500469018483958]
Robot Arm 1 togoal: 16.4722775926853
Mobile Platform 2 Pose: [-21.4431009158508;-6.09831830885473;-7.26068269900483]
Robot Arm 2 end: [-6.30492565202695;-7.89334334875726;0.30864014212197]
Robot Arm 1 togoal: 4.4228263710288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.806667e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.552350e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-5.05476589273528;-2.44208604654434;-0.592285743821928]
Robot Arm 1 end: [-3.0610609950646;-0.0924587588769462;0.455716157135655]
Robot Arm 1 togoal: 16.5403708477863
Mobile Platform 2 Pose: [-21.7472476564997;-6.12976592811478;-7.37202628471813]
Robot Arm 2 end: [-6.4753886636087;-7.97718731656965;0.335393689967894]
Robot Arm 1 togoal: 4.2332397223938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.730582e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.844038e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-4.92454767115393;-2.52355369367324;-0.571184784229672]
Robot Arm 1 end: [-3.12951677458356;-0.0969283368529976;0.378735839757613]
Robot Arm 1 togoal: 16.602317307695
Mobile Platform 2 Pose: [-22.0301071026394;-6.24700754606464;-7.51238417886988]
Robot Arm 2 end: [-6.71385920981006;-8.07427806753644;0.411004131164433]
Robot Arm 1 togoal: 3.9672682141435

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.593409e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.439276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-4.78239304130661;-2.61210051893509;-0.552463881621206]
Robot Arm 1 end: [-3.18290682557287;-0.111426481886309;0.267953636057469]
Robot Arm 1 togoal: 16.6613355725319
Mobile Platform 2 Pose: [-22.0824785829558;-6.48242416338122;-7.61325551465683]
Robot Arm 2 end: [-7.04613805625181;-8.0618550852067;0.332705537105882]
Robot Arm 1 togoal: 3.72743206832905

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.368722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.029891e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-4.59231110453075;-2.7296671739877;-0.531034560857744]
Robot Arm 1 end: [-3.2375889195588;-0.128965962767423;0.144738434719102]
Robot Arm 1 togoal: 16.7249456732809
Mobile Platform 2 Pose: [-23.1630696318185;-5.99001467185839;-7.56536948174831]
Robot Arm 2 end: [-6.43304817968;-8.07569380034257;0.280271315009438]
Robot Arm 1 togoal: 4.23857373514754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.685216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001988e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-4.26623471081008;-2.92975101969966;-0.494601632319744]
Robot Arm 1 end: [-3.39075076407971;-0.0942718280486256;0.0792720830109693]
Robot Arm 1 togoal: 16.831075697972
Mobile Platform 2 Pose: [-24.0260776079758;-5.65316168152581;-7.52951828927993]
Robot Arm 2 end: [-6.02247786898466;-8.1205354692345;0.335427840154962]
Robot Arm 1 togoal: 4.55616621418951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.167013e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.181698e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-3.99382612162951;-3.09437267528069;-0.459867696972365]
Robot Arm 1 end: [-3.51350912928767;-0.0259985345467713;0.0617431710535143]
Robot Arm 1 togoal: 16.8897899955832
Mobile Platform 2 Pose: [-24.8244152115127;-5.36804498928956;-7.50421078548338]
Robot Arm 2 end: [-5.61434353521971;-8.13353313966613;0.280271315009438]
Robot Arm 1 togoal: 4.92514861070731

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.337110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.398279e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-3.77248541911656;-3.22583418728815;-0.426992750847743]
Robot Arm 1 end: [-3.6330604309897;0.0476139138203149;0.0791082287161302]
Robot Arm 1 togoal: 16.9407608271735
Mobile Platform 2 Pose: [-25.8061220553017;-4.93731250094747;-7.45625548073163]
Robot Arm 2 end: [-5.08158732367279;-8.09809286730612;0.237626273914238]
Robot Arm 1 togoal: 5.42725364405012

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.168041e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.871836e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-3.61369573318082;-3.31943331064878;-0.400056739258697]
Robot Arm 1 end: [-3.81424550865098;0.0264574840761702;0.117837596201159]
Robot Arm 1 togoal: 17.0960169051642
Mobile Platform 2 Pose: [-26.6917149253024;-4.52920531820278;-7.36764115731803]
Robot Arm 2 end: [-4.54694098734499;-8.05883069930036;0.310978103457341]
Robot Arm 1 togoal: 5.9133868332445

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.494953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-3.48162245357287;-3.40316293340058;-0.377652926127117]
Robot Arm 1 end: [-3.78252341454568;0.108442049921789;0.0715544983284059]
Robot Arm 1 togoal: 17.0265017726652
Mobile Platform 2 Pose: [-27.4059080031854;-4.28105718413013;-7.33726841366535]
Robot Arm 2 end: [-4.20099715306542;-8.04916330947673;0.333656309696528]
Robot Arm 1 togoal: 6.23249411156967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.834557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-3.30623038539923;-3.50486147324221;-0.347648257694853]
Robot Arm 1 end: [-3.96211164996499;0.0847934005874831;0.0973732820612234]
Robot Arm 1 togoal: 17.1836723859352
Mobile Platform 2 Pose: [-28.0953837897398;-4.03691728899869;-7.28345659701393]
Robot Arm 2 end: [-3.96339083137238;-8.00643884626184;0.378892885281668]
Robot Arm 1 togoal: 6.45905140016265

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.722937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-3.16679144958357;-3.59281281700442;-0.322390703144027]
Robot Arm 1 end: [-4.05258061971745;0.108424136396946;0.0884859242493847]
Robot Arm 1 togoal: 17.2444307449187
Mobile Platform 2 Pose: [-28.8048441134116;-3.78962023853385;-7.24679368888603]
Robot Arm 2 end: [-3.66825970582156;-7.96011615874161;0.331820106629343]
Robot Arm 1 togoal: 6.75767785985297

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.103745e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-2.9931160629812;-3.69806737240174;-0.295833982316921]
Robot Arm 1 end: [-4.14587848129481;0.160190135704716;0.0941538716890567]
Robot Arm 1 togoal: 17.2905646537908
Mobile Platform 2 Pose: [-29.4486322255235;-3.54363166830307;-7.20218849501201]
Robot Arm 2 end: [-3.54746929920819;-7.92329858223039;0.34904980095441]
Robot Arm 1 togoal: 6.87904851649341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.565972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-2.84320861002757;-3.78687736860783;-0.272569349505378]
Robot Arm 1 end: [-4.22324199839052;0.208032420304551;0.104158743829802]
Robot Arm 1 togoal: 17.3260116925902
Mobile Platform 2 Pose: [-30.1203886919227;-3.30254485212615;-7.13602848345275]
Robot Arm 2 end: [-3.18848727791874;-7.8708917453693;0.412223357441178]
Robot Arm 1 togoal: 7.22213217589483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.671582e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-2.70684096395829;-3.86652855623136;-0.250841983083456]
Robot Arm 1 end: [-4.29189911300247;0.249507639084128;0.112379491023336]
Robot Arm 1 togoal: 17.3584185130682
Mobile Platform 2 Pose: [-30.6738897262103;-3.08590155724162;-7.13098347395578]
Robot Arm 2 end: [-3.10492038402497;-7.87128947583776;0.406863685218259]
Robot Arm 1 togoal: 7.30170056454172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.753813e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-2.57840423843356;-3.94073817948405;-0.229912595308235]
Robot Arm 1 end: [-4.35467789720736;0.286810735061306;0.118259342247099]
Robot Arm 1 togoal: 17.3888040009084
Mobile Platform 2 Pose: [-31.3603585695149;-2.84136130720597;-7.07902131344822]
Robot Arm 2 end: [-2.74454769799076;-7.80597867722438;0.394554027371353]
Robot Arm 1 togoal: 7.66317154974209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.819837e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-2.45507848285763;-4.01133335909876;-0.209400677029599]
Robot Arm 1 end: [-4.41352872540532;0.321583171498012;0.12257777976863]
Robot Arm 1 togoal: 17.4177250905608
Mobile Platform 2 Pose: [-31.924226511813;-2.62391717003488;-7.07583787381631]
Robot Arm 2 end: [-2.6521616627503;-7.80963726311912;0.403304498735894]
Robot Arm 1 togoal: 7.74839727834207

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.876993e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-2.33554388299452;-4.07917699323183;-0.189128812084401]
Robot Arm 1 end: [-4.46952546094854;0.354778018573605;0.126083362476609]
Robot Arm 1 togoal: 17.4454368406661
Mobile Platform 2 Pose: [-32.6217254102042;-2.3782294740693;-7.02960416259606]
Robot Arm 2 end: [-2.29369958228788;-7.7389839684167;0.357406498452791]
Robot Arm 1 togoal: 8.1149990425107

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.929272e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-2.21917128053166;-4.1446945995829;-0.169019922495243]
Robot Arm 1 end: [-4.52323029000953;0.386846124950694;0.129218678535907]
Robot Arm 1 togoal: 17.4720920535808
Mobile Platform 2 Pose: [-33.1971984344018;-2.160949501248;-7.0271035439853]
Robot Arm 2 end: [-2.19635353009899;-7.74951543940588;0.390197127861396]
Robot Arm 1 togoal: 8.20001793414551

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.978270e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-2.10556155351118;-4.20816176784523;-0.149032695977321]
Robot Arm 1 end: [-4.57494424243187;0.417976793819395;0.132154841189211]
Robot Arm 1 togoal: 17.4978015553079
Mobile Platform 2 Pose: [-33.9136576203659;-1.91380427107469;-6.9894308057932]
Robot Arm 2 end: [-1.83758725167947;-7.69328840073237;0.357507682113972]
Robot Arm 1 togoal: 8.56151941245952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.024283e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-1.99432755975603;-4.26983620494211;-0.129125587191545]
Robot Arm 1 end: [-4.6248441808564;0.448272964544093;0.134894538958385]
Robot Arm 1 togoal: 17.5226380725996
Mobile Platform 2 Pose: [-34.4720484214752;-1.70723703020531;-6.99985295441918]
Robot Arm 2 end: [-1.75845519849862;-7.71923655735973;0.387058058192418]
Robot Arm 1 togoal: 8.62616756873115

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.067119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-1.88502509997469;-4.32999786644568;-0.109241719416542]
Robot Arm 1 end: [-4.6730458983529;0.477847777327681;0.137368203070361]
Robot Arm 1 togoal: 17.5466263377434
Mobile Platform 2 Pose: [-35.2227835123194;-1.44249835234897;-6.96083650478427]
Robot Arm 2 end: [-1.34011806115012;-7.65124742490695;0.340022357706179]
Robot Arm 1 togoal: 9.05013270871883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.106583e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-1.7771578121118;-4.38894488188558;-0.0893064365055577]
Robot Arm 1 end: [-4.71962873867747;0.506860054700838;0.139500242115331]
Robot Arm 1 togoal: 17.5697386948859
Mobile Platform 2 Pose: [-35.5106359096292;-1.3277927656028;-6.9764746020853]
Robot Arm 2 end: [-1.36003311101829;-7.69754280883086;0.359535108165492]
Robot Arm 1 togoal: 9.01661453400504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.142679e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.249313e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-1.6702065226586;-4.44697550151756;-0.0692304920776147]
Robot Arm 1 end: [-4.7646437822806;0.535513037985692;0.14124413942532]
Robot Arm 1 togoal: 17.5919000027446
Mobile Platform 2 Pose: [-35.9283142224813;-1.16093485940719;-7.00095299541926]
Robot Arm 2 end: [-1.25833407748544;-7.69907480848189;0.314923105914561]
Robot Arm 1 togoal: 9.11951760304578

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.175639e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.153760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-1.56365779390428;-4.50437096969885;-0.048914413402279]
Robot Arm 1 end: [-4.80811581090317;0.564036762043324;0.142594155311352]
Robot Arm 1 togoal: 17.6129980062981
Mobile Platform 2 Pose: [-36.4615395516019;-0.952039400921655;-6.98749685843634]
Robot Arm 2 end: [-0.968991087211805;-7.70775148456441;0.356011270986165]
Robot Arm 1 togoal: 9.38992672911273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.205872e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.217480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-1.45702235868867;-4.56138449363226;-0.0282518587179328]
Robot Arm 1 end: [-4.85004239244103;0.592666981822348;0.143582728694677]
Robot Arm 1 togoal: 17.6328940310852
Mobile Platform 2 Pose: [-36.8384524973832;-0.800355569649052;-7.00517685963609]
Robot Arm 2 end: [-0.909334443875014;-7.70562702181168;0.319851015810221]
Robot Arm 1 togoal: 9.45235983755263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-1.34984194974671;-4.61823682401939;-0.00713113077523344]
Robot Arm 1 end: [-4.89039085344916;0.621626916346982;0.144270637870198]
Robot Arm 1 togoal: 17.6514306842596
Mobile Platform 2 Pose: [-37.4514110243285;-0.555527947964906;-6.95458599424949]
Robot Arm 2 end: [-0.539134439176838;-7.70076621636616;0.429469804180429]
Robot Arm 1 togoal: 9.79723896412448

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-1.24168543847357;-4.67511752709069;0.0145654681121181]
Robot Arm 1 end: [-4.92909354004907;0.651113935412674;0.144734545952062]
Robot Arm 1 togoal: 17.6684354478877
Mobile Platform 2 Pose: [-37.9716647912377;-0.354679744542765;-6.95686286366955]
Robot Arm 2 end: [-0.3313073600424;-7.6847416469542;0.368371788719276]
Robot Arm 1 togoal: 10.0086257998168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-1.13213546473397;-4.73219080359392;0.0369646907125699]
Robot Arm 1 end: [-4.96604182217116;0.681291265991941;0.145054774712429]
Robot Arm 1 togoal: 17.6837203539087
Mobile Platform 2 Pose: [-38.4856794198526;-0.15614541881859;-6.94757013312022]
Robot Arm 2 end: [-0.143997875535254;-7.67125892243134;0.359373842503993]
Robot Arm 1 togoal: 10.1937818405922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-1.02076630406597;-4.7896048091817;0.0602082122639586]
Robot Arm 1 end: [-5.00107926919119;0.712283910437902;0.145305113121002]
Robot Arm 1 togoal: 17.6970784544414
Mobile Platform 2 Pose: [-38.9868138861594;0.0370763969987037;-6.93720581893308]
Robot Arm 2 end: [0.021369878643755;-7.67270950993988;0.402564297257234]
Robot Arm 1 togoal: 10.3486730400995

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.669239e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-0.907113365117983;-4.84750337604327;0.0844619752348241]
Robot Arm 1 end: [-5.03399379316504;0.744177556598861;0.145545780752181]
Robot Arm 1 togoal: 17.7082775217111
Mobile Platform 2 Pose: [-39.5339869391334;0.245540643888389;-6.93244009886757]
Robot Arm 2 end: [0.293475348890041;-7.65104520958683;0.360024076627015]
Robot Arm 1 togoal: 10.6217271881867

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.116884e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-0.790634238081913;-4.90603885652787;0.109930123413547]
Robot Arm 1 end: [-5.06450783175337;0.777018814392894;0.145820096532911]
Robot Arm 1 togoal: 17.71705105061
Mobile Platform 2 Pose: [-39.846209352466;0.37175701454472;-6.94449763305896]
Robot Arm 2 end: [0.282535529301645;-7.66312632530581;0.351011261697697]
Robot Arm 1 togoal: 10.6094481067859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-0.670660317932425;-4.96538450816082;0.136875478429964]
Robot Arm 1 end: [-5.09226456221534;0.810814677246982;0.14615484689955]
Robot Arm 1 togoal: 17.7230859532646
Mobile Platform 2 Pose: [-40.2736552156421;0.538987278078321;-6.95405884022436]
Robot Arm 2 end: [0.472996996054379;-7.66782742948965;0.344283693304296]
Robot Arm 1 togoal: 10.7938573385259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-0.54633634779193;-5.0257442199623;0.165650177857556]
Robot Arm 1 end: [-5.11680708142116;0.845528647005476;0.146563871552588]
Robot Arm 1 togoal: 17.7260056628921
Mobile Platform 2 Pose: [-40.7916536563221;0.736682423582564;-6.93380967842036]
Robot Arm 2 end: [0.742576495778158;-7.66438460128629;0.391709544244279]
Robot Arm 1 togoal: 11.0513956580974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-0.43011567265361;-5.08056914475715;0.196743410990028]
Robot Arm 1 end: [-5.27214709134518;0.858348938166812;0.150720154617712]
Robot Arm 1 togoal: 17.8517765998418
Mobile Platform 2 Pose: [-41.2626394828068;0.91732377686777;-6.94065310718704]
Robot Arm 2 end: [0.918163960914753;-7.65853348922686;0.344113424676616]
Robot Arm 1 togoal: 11.2282604044289

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-0.34724011458642;-5.12164786500442;0.219238747290446]
Robot Arm 1 end: [-5.30544141691742;0.883950780629072;0.150874539079469]
Robot Arm 1 togoal: 17.8671803627586
Mobile Platform 2 Pose: [-41.6633271689439;1.07873501849655;-6.95066485880393]
Robot Arm 2 end: [0.990834206572472;-7.65443278319906;0.320731498963128]
Robot Arm 1 togoal: 11.3022460864855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-0.25760924396871;-5.16579180917106;0.241363043643067]
Robot Arm 1 end: [-5.33050983095365;0.909273536178741;0.147995014781912]
Robot Arm 1 togoal: 17.8759889798137
Mobile Platform 2 Pose: [-42.1532377821149;1.26849668933945;-6.93174470295168]
Robot Arm 2 end: [1.25448846772328;-7.65384588343897;0.343273812010407]
Robot Arm 1 togoal: 11.5565993606532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-0.196448281949153;-5.19546166387155;0.266107261312396]
Robot Arm 1 end: [-5.33348674272468;0.940091778878592;0.148461850545891]
Robot Arm 1 togoal: 17.8628556965611
Mobile Platform 2 Pose: [-42.5568237117988;1.42809222617238;-6.93254286572036]
Robot Arm 2 end: [1.35486134393007;-7.64753610379209;0.336856520232906]
Robot Arm 1 togoal: 11.6562926483301

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.783538e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-0.0846151521740746;-5.24968595276984;0.292807668448217]
Robot Arm 1 end: [-5.28425767854616;0.996097810919607;0.16990816137561]
Robot Arm 1 togoal: 17.7905659429688
Mobile Platform 2 Pose: [-43.0000052247388;1.60507006735511;-6.93340486618106]
Robot Arm 2 end: [1.53995026053779;-7.65305031167899;0.343196372026339]
Robot Arm 1 togoal: 11.8349417506494

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.755751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [0.0587912140199154;-5.31665222207269;0.326165607461577]
Robot Arm 1 end: [-5.22136296330878;1.01414333430252;0.163799079086966]
Robot Arm 1 togoal: 17.7278736688012
Mobile Platform 2 Pose: [-43.4620372568128;1.78311615790777;-6.91047501283759]
Robot Arm 2 end: [1.76695608795263;-7.65746989640385;0.414341505326793]
Robot Arm 1 togoal: 12.0487922998901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.726542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [0.262176230660571;-5.40408876528445;0.386522723799857]
Robot Arm 1 end: [-5.17514061302904;1.01015368889106;0.141312330427489]
Robot Arm 1 togoal: 17.6919690082289
Mobile Platform 2 Pose: [-43.8847982940213;1.95196334782286;-6.91442191919358]
Robot Arm 2 end: [1.93308759727068;-7.65832749909149;0.398223310736959]
Robot Arm 1 togoal: 12.2123968654642

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.856380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [0.487511774876881;-5.46101317993625;0.541771038855473]
Robot Arm 1 end: [-5.34933046283463;1.14400191584156;0.115300799833048]
Robot Arm 1 togoal: 17.776575541011
Mobile Platform 2 Pose: [-44.308252559885;2.12280563664199;-6.91606635760097]
Robot Arm 2 end: [2.08169402003931;-7.65948446801274;0.395332892615308]
Robot Arm 1 togoal: 12.3576886910933

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967820e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [0.709113824370352;-5.5461276599734;0.62142230507504]
Robot Arm 1 end: [-5.55609054784224;1.25497165061771;0.162373628983417]
Robot Arm 1 togoal: 17.8972997846096
Mobile Platform 2 Pose: [-44.7228501570528;2.28616429949893;-6.91186326502394]
Robot Arm 2 end: [2.27859234048091;-7.66245873287768;0.416155044697374]
Robot Arm 1 togoal: 12.5478528607278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008104e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [0.493210397420372;-5.36981990101452;0.851595667050572]
Robot Arm 1 end: [-5.54367278849736;1.47248570039518;0.212702438509321]
Robot Arm 1 togoal: 17.7773987522146
Mobile Platform 2 Pose: [-44.9670619445186;2.39026062175475;-6.92256039954616]
Robot Arm 2 end: [2.25698927244087;-7.66626642226657;0.417821813810627]
Robot Arm 1 togoal: 12.5258572779293

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.785166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002936e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-0.0212488763355969;-5.0649352745139;0.972879521724861]
Robot Arm 1 end: [-5.33064381604083;1.60455755340319;0.308972396307424]
Robot Arm 1 togoal: 17.520876250934
Mobile Platform 2 Pose: [-45.362795551343;2.55448786528037;-6.93588022290134]
Robot Arm 2 end: [2.47153615259317;-7.66955861254908;0.37177614505545]
Robot Arm 1 togoal: 12.7393440386292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.753965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002857e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-0.476226774380316;-4.82973201949;1.06590232590282]
Robot Arm 1 end: [-5.25515941660935;1.67926072237755;0.476235364049411]
Robot Arm 1 togoal: 17.4082200151787
Mobile Platform 2 Pose: [-45.8298694630363;2.7418069417981;-6.92040158423829]
Robot Arm 2 end: [2.73135606242266;-7.67623363410074;0.424803781290102]
Robot Arm 1 togoal: 12.9880315468274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.903057e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002513e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-0.47453087896662;-4.83335092456112;1.22261200427754]
Robot Arm 1 end: [-5.22435832837808;1.84340714055577;0.437705469474075]
Robot Arm 1 togoal: 17.3056239594375
Mobile Platform 2 Pose: [-46.1958777380149;2.8933175535402;-6.92766625236752]
Robot Arm 2 end: [2.8516732264215;-7.67978840297673;0.403608334719954]
Robot Arm 1 togoal: 13.10715265446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.135123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002636e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-0.592819423995656;-4.76481250238463;1.25159996762085]
Robot Arm 1 end: [-5.22311736387029;1.80991198617437;0.395912460485355]
Robot Arm 1 togoal: 17.3230097246394
Mobile Platform 2 Pose: [-46.5839602269065;3.05045067780769;-6.9253886152149]
Robot Arm 2 end: [3.02615379550644;-7.68264811943602;0.412696392648036]
Robot Arm 1 togoal: 13.2770154526011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.786085e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002590e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-0.698936416843658;-4.70280022504712;1.25506281333282]
Robot Arm 1 end: [-5.2140216121918;1.80030083855414;0.483297033650265]
Robot Arm 1 togoal: 17.3141083339809
Mobile Platform 2 Pose: [-46.9499378501976;3.20107531528331;-6.92703750982503]
Robot Arm 2 end: [3.15960657397908;-7.68487461187891;0.410256255668395]
Robot Arm 1 togoal: 13.4077888710334

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.876230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002553e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-0.796882395114995;-4.64640697468221;1.26256408266804]
Robot Arm 1 end: [-5.21118906252329;1.77619418629965;0.635146810368383]
Robot Arm 1 togoal: 17.3146178309214
Mobile Platform 2 Pose: [-47.3236359762603;3.35401481878709;-6.92510474992747]
Robot Arm 2 end: [3.31659988028209;-7.6882481804812;0.42330371420322]
Robot Arm 1 togoal: 13.5602716043016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.184992e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-0.893206410616244;-4.59222134298169;1.30023451732532]
Robot Arm 1 end: [-5.19262765770612;1.74869185262225;0.786199523375659]
Robot Arm 1 togoal: 17.3043333598589
Mobile Platform 2 Pose: [-47.6549222767326;3.48847724994262;-6.92562376339646]
Robot Arm 2 end: [3.42892543670975;-7.69185459300309;0.438022853796099]
Robot Arm 1 togoal: 13.668811693185

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.575017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002090e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-0.875980093424254;-4.60265945354612;1.30938990948864]
Robot Arm 1 end: [-5.21639102981159;1.7200566699921;0.875825584955521]
Robot Arm 1 togoal: 17.3352896585578
Mobile Platform 2 Pose: [-47.9999924977581;3.63154596192378;-6.92859192470702]
Robot Arm 2 end: [3.59251302018961;-7.69611845153254;0.427399542912029]
Robot Arm 1 togoal: 13.8296869940406

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.910642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002259e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-0.852023941910652;-4.61679331524552;1.3149023841321]
Robot Arm 1 end: [-5.21992041419987;1.67554526892223;0.989598276347313]
Robot Arm 1 togoal: 17.3558322121482
Mobile Platform 2 Pose: [-48.3180404813151;3.7610179220817;-6.92833856101665]
Robot Arm 2 end: [3.70693308521774;-7.69956442238492;0.439697153677117]
Robot Arm 1 togoal: 13.9406324926404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.237853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001340e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-0.802289773233327;-4.64534189217939;1.31951531264812]
Robot Arm 1 end: [-5.19714021170959;1.62398772796412;1.11797173137467]
Robot Arm 1 togoal: 17.3572199381401
Mobile Platform 2 Pose: [-48.635674083177;3.89068124827271;-6.92791994401125]
Robot Arm 2 end: [3.82575852120712;-7.70469584711611;0.463692065162303]
Robot Arm 1 togoal: 14.054818387658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.161501e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000797e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-0.651123183545916;-4.73158024296508;1.32064078369666]
Robot Arm 1 end: [-5.18594494836899;1.53778500149049;1.24439111950415]
Robot Arm 1 togoal: 17.3867339983294
Mobile Platform 2 Pose: [-48.9491548763659;4.01957371028054;-6.92942674813952]
Robot Arm 2 end: [3.95742626983568;-7.71148816492592;0.482615501878821]
Robot Arm 1 togoal: 14.1818643648539

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.884332e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-1.22038994573751;-4.44312365432477;1.16970716814477]
Robot Arm 1 end: [-4.96692562823671;1.54111310053043;1.14026075018216]
Robot Arm 1 togoal: 17.1961214439634
Mobile Platform 2 Pose: [-49.2505122769299;4.1445035057339;-6.93302503383492]
Robot Arm 2 end: [4.05812721604392;-7.71692034348757;0.508568652577092]
Robot Arm 1 togoal: 14.2782495696671

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.647413e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001871e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-1.40875532617856;-4.33715165227588;1.16269794791725]
Robot Arm 1 end: [-4.94020596479511;1.58781186279401;1.02623716282084]
Robot Arm 1 togoal: 17.1528264082425
Mobile Platform 2 Pose: [-49.5613194709434;4.27336269782278;-6.93772530741384]
Robot Arm 2 end: [4.22040543166493;-7.7272370118989;0.512972845294705]
Robot Arm 1 togoal: 14.4361179333514

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.234150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-1.53108784492977;-4.26669494513799;1.16303764126866]
Robot Arm 1 end: [-4.93969136874269;1.567189417848;0.955712050911249]
Robot Arm 1 togoal: 17.1646795185091
Mobile Platform 2 Pose: [-49.8584052742476;4.39987663950843;-6.94452850950977]
Robot Arm 2 end: [4.3199522920336;-7.73393067736702;0.517792724931665]
Robot Arm 1 togoal: 14.5328086981783

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.726034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-1.61973998394011;-4.21529858439255;1.16593464351225]
Robot Arm 1 end: [-4.95116455813829;1.52602628003452;0.883698725260438]
Robot Arm 1 togoal: 17.1974330981685
Mobile Platform 2 Pose: [-50.1642003746435;4.52896749630525;-6.94958284722997]
Robot Arm 2 end: [4.44846343901664;-7.74199633041486;0.534186236732618]
Robot Arm 1 togoal: 14.65709649705

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.145907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-1.68990263525353;-4.17359684276557;1.16937723282447]
Robot Arm 1 end: [-4.96351263091734;1.46564912645371;0.806941524704134]
Robot Arm 1 togoal: 17.2409899540184
Mobile Platform 2 Pose: [-50.4457525093769;4.64961712096214;-6.95806286794638]
Robot Arm 2 end: [4.55382336077013;-7.74994902969787;0.533097471859348]
Robot Arm 1 togoal: 14.7598257168085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.509356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-1.73588832530706;-4.14396237969726;1.1732469958594]
Robot Arm 1 end: [-4.97340712205136;1.39073101841871;0.724667660133318]
Robot Arm 1 togoal: 17.2903608532452
Mobile Platform 2 Pose: [-50.7264747896218;4.76961292075548;-6.9659403619208]
Robot Arm 2 end: [4.81595897518909;-7.74691044288721;0.447903574184283]
Robot Arm 1 togoal: 15.0246660144562

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.843078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-1.7407424077568;-4.15234840941424;1.17685970105118]
Robot Arm 1 end: [-5.00095455092127;1.30551517437776;0.637678853810892]
Robot Arm 1 togoal: 17.3609608048946
Mobile Platform 2 Pose: [-50.9224981397522;4.85264421389842;-6.9707756749295]
Robot Arm 2 end: [5.02660800436527;-7.68726165192216;0.311578492721665]
Robot Arm 1 togoal: 15.2515706458644

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.522871e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.865369e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-1.71549575208066;-4.1791587868628;1.16393360062658]
Robot Arm 1 end: [-4.99170585830543;1.39042573170367;0.549048070172717]
Robot Arm 1 togoal: 17.3153314775419
Mobile Platform 2 Pose: [-51.1064287151198;4.88387409970099;-6.94763182431124]
Robot Arm 2 end: [5.14567821018874;-7.6052713281707;0.253473992326818]
Robot Arm 1 togoal: 15.3861273892554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.494959e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.834755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-1.74463885200222;-4.161806352333;1.17520507736628]
Robot Arm 1 end: [-4.94888995976009;1.40988709312914;0.42770315333632]
Robot Arm 1 togoal: 17.2758400310008
Mobile Platform 2 Pose: [-51.1304856438837;4.88515262658776;-6.94576138478439]
Robot Arm 2 end: [5.06748424859762;-7.59992682778002;0.188122801420646]
Robot Arm 1 togoal: 15.3155474743882

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005198e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.737106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-1.78524212042678;-4.14284383290118;1.20338502922462]
Robot Arm 1 end: [-4.91621720942015;1.34194199248884;0.311220570083091]
Robot Arm 1 togoal: 17.289276172503
Mobile Platform 2 Pose: [-51.0491659946115;4.81217908939481;-6.92362120996902]
Robot Arm 2 end: [4.87839952793405;-7.60939529447895;0.163099411955314]
Robot Arm 1 togoal: 15.1302894016572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006371e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.664281e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-1.7993852963111;-4.15836879484316;1.23615039307742]
Robot Arm 1 end: [-4.90114002847469;1.23460528250663;0.228349622227018]
Robot Arm 1 togoal: 17.3362932514661
Mobile Platform 2 Pose: [-51.0158840878525;4.73669056772936;-6.85724007321117]
Robot Arm 2 end: [4.66763080980384;-7.54386899260685;0.135137537986219]
Robot Arm 1 togoal: 14.9362842722614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006614e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.797306e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-1.7904100873594;-4.21492741710585;1.26413590262926]
Robot Arm 1 end: [-4.94632930570712;1.14094644958067;0.231958846331296]
Robot Arm 1 togoal: 17.4223194959709
Mobile Platform 2 Pose: [-51.1654353190257;4.77906384793932;-6.79277861921761]
Robot Arm 2 end: [4.56812724285239;-7.53086108853126;0.231985904664232]
Robot Arm 1 togoal: 14.8320105065376

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003566e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.902280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-1.76983328918212;-4.30719025392206;1.27805913601727]
Robot Arm 1 end: [-5.03051942576813;1.10595186412629;0.240181645720781]
Robot Arm 1 togoal: 17.5117418353598
Mobile Platform 2 Pose: [-51.3840331098792;4.86729097896288;-6.75386341000832]
Robot Arm 2 end: [4.502356506452;-7.48113333501817;0.312324675688039]
Robot Arm 1 togoal: 14.7690192482706

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.304554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-1.70951097309556;-4.43383453220596;1.27891006034915]
Robot Arm 1 end: [-5.12943127211424;1.12388110730208;0.196923892354934]
Robot Arm 1 togoal: 17.5908599776638
Mobile Platform 2 Pose: [-51.5944847483408;4.94454630860854;-6.73513661676336]
Robot Arm 2 end: [4.46793678726653;-7.36366953473751;0.249576327330297]
Robot Arm 1 togoal: 14.7610281267206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.559525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-1.48031866540859;-4.6290794963445;1.25959641318076]
Robot Arm 1 end: [-5.34545044937657;1.08543374958809;0.262052349086666]
Robot Arm 1 togoal: 17.7914948814032
Mobile Platform 2 Pose: [-51.3246983523427;4.77036219136908;-6.77621948227637]
Robot Arm 2 end: [4.20684164429309;-7.32737835700925;0.256199010358548]
Robot Arm 1 togoal: 14.5112707057653

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.355558e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-1.21452341994478;-4.7880122768061;1.22516309585756]
Robot Arm 1 end: [-5.50472179655261;1.05339700422732;0.293992062063164]
Robot Arm 1 togoal: 17.9427882903472
Mobile Platform 2 Pose: [-51.2910402004511;4.73893753437245;-6.77723970557762]
Robot Arm 2 end: [4.03349313175144;-7.25147075868943;0.471966437013221]
Robot Arm 1 togoal: 14.3385429484842

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.499979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-0.919175760797696;-4.95919789290712;1.18348849722878]
Robot Arm 1 end: [-5.66508187038258;1.01571091306085;0.295980669837427]
Robot Arm 1 togoal: 18.1000804628807
Mobile Platform 2 Pose: [-51.1008271581904;4.48837682272819;-6.83547148798347]
Robot Arm 2 end: [3.71715398282006;-7.21813677438571;0.513048584862769]
Robot Arm 1 togoal: 14.0326420338331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.951001e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-0.598584146058194;-5.1387650643355;1.13294949958997]
Robot Arm 1 end: [-5.84444504024285;0.973504577656377;0.314250383880063]
Robot Arm 1 togoal: 18.2751279852939
Mobile Platform 2 Pose: [-50.8443916394863;4.30960058297514;-6.86701983967481]
Robot Arm 2 end: [3.60834482232825;-7.36730656298827;0.723526850849564]
Robot Arm 1 togoal: 13.8835905644141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.517001e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-0.28846344144284;-5.31531795422434;1.08318664192895]
Robot Arm 1 end: [-6.01474847472785;0.927914604332637;0.313321205484858]
Robot Arm 1 togoal: 18.445416509415
Mobile Platform 2 Pose: [-50.4144229891725;3.94426702610888;-6.97228947906889]
Robot Arm 2 end: [3.39603454561388;-7.55683602298458;0.931113661111849]
Robot Arm 1 togoal: 13.6297746738042

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.933243e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [0.00854190305759012;-5.48532859378998;1.03483982463788]
Robot Arm 1 end: [-6.17853924196979;0.856073943304654;0.32348033474391]
Robot Arm 1 togoal: 18.6223137413949
Mobile Platform 2 Pose: [-50.7505087666298;4.06220494140766;-6.96944750531736]
Robot Arm 2 end: [3.39681507787364;-7.48032890929275;0.831456341456136]
Robot Arm 1 togoal: 13.6491342983036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.443455e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [0.298767282729591;-5.65077053061689;0.984025767493166]
Robot Arm 1 end: [-6.3279946825594;0.764111017779688;0.322295657973717]
Robot Arm 1 togoal: 18.7973920463765
Mobile Platform 2 Pose: [-51.0510855886734;4.16760907937658;-6.97172844619892]
Robot Arm 2 end: [3.41454277675101;-7.34382224013877;0.756228596247193]
Robot Arm 1 togoal: 13.6963540221937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [0.605395587141431;-5.81973851245528;0.919617720732016]
Robot Arm 1 end: [-6.47668638382126;0.647654769571877;0.326749224414823]
Robot Arm 1 togoal: 18.9835136477655
Mobile Platform 2 Pose: [-51.3549719807613;4.27756582653588;-6.97898364762219]
Robot Arm 2 end: [3.48859991114986;-7.22386754968634;0.677198110594589]
Robot Arm 1 togoal: 13.7971461024851

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [0.927766190442175;-5.98752587117536;0.833197053957811]
Robot Arm 1 end: [-6.61409209944586;0.506300677614537;0.32228640097649]
Robot Arm 1 togoal: 19.1727749010209
Mobile Platform 2 Pose: [-51.7126996344453;4.40605329465317;-7.00319599693809]
Robot Arm 2 end: [3.60814119394897;-6.9093511279926;0.65194369883509]
Robot Arm 1 togoal: 13.9817336571359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.377256e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [1.24593825172578;-6.1298197055645;0.706437305699074]
Robot Arm 1 end: [-6.72652394099272;0.326808995518479;0.328911454087638]
Robot Arm 1 togoal: 19.3589334018969
Mobile Platform 2 Pose: [-51.8444151479915;4.25900766409278;-7.1438460001778]
Robot Arm 2 end: [3.51144833150895;-6.9449173389122;0.686133638885915]
Robot Arm 1 togoal: 13.8776715598127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [1.4764423770106;-6.19369214464475;0.543932027578446]
Robot Arm 1 end: [-6.76452556904883;0.137199135112645;0.318783291901527]
Robot Arm 1 togoal: 19.4876751683967
Mobile Platform 2 Pose: [-51.5314874545494;4.01576899928004;-7.29224870830285]
Robot Arm 2 end: [3.21610816931498;-7.38182247088965;0.671284525954378]
Robot Arm 1 togoal: 13.4997179638615

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [1.51984316350657;-6.12540770469262;0.373266290674307]
Robot Arm 1 end: [-6.69349763072719;-0.0698255174370665;0.346433898426359]
Robot Arm 1 togoal: 19.530843674191
Mobile Platform 2 Pose: [-51.9300483400978;4.16448834130083;-7.31880190261299]
Robot Arm 2 end: [3.38949575363007;-7.30157753955015;0.751752414704003]
Robot Arm 1 togoal: 13.6803443728227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.903050e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [1.35842900765045;-5.93885642531868;0.237752805852848]
Robot Arm 1 end: [-6.57690666966347;-0.139577205285179;0.432238784203254]
Robot Arm 1 togoal: 19.4625348200509
Mobile Platform 2 Pose: [-52.4744512795214;4.33366566571294;-7.36740858088024]
Robot Arm 2 end: [3.61882073378426;-7.11368263117476;0.582652013282773]
Robot Arm 1 togoal: 13.9529066175013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.665484e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [1.09056472678927;-5.71192626088101;0.13285904784842]
Robot Arm 1 end: [-6.39473571208445;-0.154893787535111;0.424784331679485]
Robot Arm 1 togoal: 19.3160792009323
Mobile Platform 2 Pose: [-52.499275865357;4.0475817941879;-7.47131238247069]
Robot Arm 2 end: [3.39544524081824;-7.08947813955087;0.596441120995599]
Robot Arm 1 togoal: 13.7391375209894

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.588400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [0.778764164977383;-5.48633141419905;0.0638113553362711]
Robot Arm 1 end: [-6.21358032010182;-0.127403776510912;0.484276782174193]
Robot Arm 1 togoal: 19.1446934966131
Mobile Platform 2 Pose: [-52.3824101100833;3.67722586929648;-7.51294272580938]
Robot Arm 2 end: [3.12680984230122;-7.01523745358614;0.820484580499406]
Robot Arm 1 togoal: 13.4800840482309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.686566e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [0.523452339954406;-5.32462648190993;0.0440411498433757]
Robot Arm 1 end: [-6.06470505181396;-0.110653758918143;0.497560279160438]
Robot Arm 1 togoal: 19.0091424503236
Mobile Platform 2 Pose: [-51.8946109217083;3.32652302380932;-7.60066215775632]
Robot Arm 2 end: [2.60970491703134;-7.42828873196041;0.935911230958424]
Robot Arm 1 togoal: 12.8825729498585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.627813e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [0.36020242882996;-5.22742730526728;0.0406816949892734]
Robot Arm 1 end: [-5.97264747629391;-0.115706218930912;0.514943135609799]
Robot Arm 1 togoal: 18.9331753840729
Mobile Platform 2 Pose: [-52.4529185070146;3.4680596101102;-7.68330055010343]
Robot Arm 2 end: [2.97411638026588;-7.15457264798127;0.874405133538847]
Robot Arm 1 togoal: 13.2982042992892

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.716775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [0.27571333352772;-5.17891041873939;0.0408703964472245]
Robot Arm 1 end: [-5.93102921546134;-0.11802157279399;0.541800950933524]
Robot Arm 1 togoal: 18.8979068467674
Mobile Platform 2 Pose: [-52.0808327518148;3.22724353841285;-7.74265527325095]
Robot Arm 2 end: [2.56620952464483;-7.44319869683425;0.954872547475233]
Robot Arm 1 togoal: 12.8361736905375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.278989e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [0.252855535804441;-5.16409111491486;0.0386389774148206]
Robot Arm 1 end: [-5.9245244825236;-0.123845140728148;0.583551112885268]
Robot Arm 1 togoal: 18.8934255984427
Mobile Platform 2 Pose: [-51.9142278595602;2.90871564386241;-7.8942490177388]
Robot Arm 2 end: [2.21384551408326;-7.7564098588907;1.03026853517944]
Robot Arm 1 togoal: 12.4278934793367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.198639e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [0.303907724514239;-5.18170650525435;0.0284862628613921]
Robot Arm 1 end: [-5.94949671880182;-0.142699514987515;0.656814510994124]
Robot Arm 1 togoal: 18.921089175674
Mobile Platform 2 Pose: [-52.3533618351413;2.99726354466114;-7.93529537647885]
Robot Arm 2 end: [2.46699510870219;-7.41719529463628;0.880757682886351]
Robot Arm 1 togoal: 12.7478138286953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.725099e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [0.476046334347974;-5.20875207388802;-0.0117959105653632]
Robot Arm 1 end: [-5.96324455053894;-0.2207698086676;0.753038580525219]
Robot Arm 1 togoal: 18.9704790873579
Mobile Platform 2 Pose: [-52.1061767719937;2.84097769391466;-7.96779543033256]
Robot Arm 2 end: [2.17733786121671;-7.64550590011637;0.955463686693328]
Robot Arm 1 togoal: 12.4157573741708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.841255e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [0.224396602925098;-5.0107294444812;-0.0111705732994244]
Robot Arm 1 end: [-5.66500457543128;-0.369855602652951;0.78287296131454]
Robot Arm 1 togoal: 18.8008272449218
Mobile Platform 2 Pose: [-51.820677339407;2.48406071596428;-8.0892025045086]
Robot Arm 2 end: [1.80980187739512;-8.09442733657916;1.11670506673984]
Robot Arm 1 togoal: 11.9693810179124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.758072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [0.020517330912976;-4.84265291728125;-0.0064618654287069]
Robot Arm 1 end: [-5.46441411914333;-0.343701002846317;0.865663369079226]
Robot Arm 1 togoal: 18.6163831230578
Mobile Platform 2 Pose: [-52.2264998318599;2.57905695292061;-8.12087839049262]
Robot Arm 2 end: [1.93795747737183;-7.74636395045856;0.965757458312069]
Robot Arm 1 togoal: 12.1614965549241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.260249e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [0.0301159071786879;-4.8099835594971;-0.0282951354293795]
Robot Arm 1 end: [-5.4032899086621;-0.309445877439131;0.949951310708941]
Robot Arm 1 togoal: 18.5437890118411
Mobile Platform 2 Pose: [-51.9112634955784;2.34824658804183;-8.178583255839]
Robot Arm 2 end: [1.63319051401665;-8.00220794397692;1.02680893344392]
Robot Arm 1 togoal: 11.8138274680469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.282751e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [0.0187253912502637;-4.78673842543063;-0.0458940909806442]
Robot Arm 1 end: [-5.28715125540379;-0.356059681673583;1.02926781319595]
Robot Arm 1 togoal: 18.471241598212
Mobile Platform 2 Pose: [-51.9499970179248;2.12117751705113;-8.28570081409928]
Robot Arm 2 end: [1.49300509963083;-8.21783929040682;1.20566810476278]
Robot Arm 1 togoal: 11.6346335231975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.456686e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [-0.0426343661116225;-4.7399882581695;-0.0574480496077005]
Robot Arm 1 end: [-5.11856270911158;-0.396098269576769;1.09276966141143]
Robot Arm 1 togoal: 18.3530155245105
Mobile Platform 2 Pose: [-52.1771919557311;1.92063867179783;-8.34713243973684]
Robot Arm 2 end: [1.18045472438734;-8.07509607405508;0.939485090978809]
Robot Arm 1 togoal: 11.359840780469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.663398e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [-0.109455808451303;-4.69426188306374;-0.063831493207455]
Robot Arm 1 end: [-4.90834199292766;-0.425717840708455;1.13495075030403]
Robot Arm 1 togoal: 18.1962437743439
Mobile Platform 2 Pose: [-51.9957058758834;1.71688497163666;-8.35750652985512]
Robot Arm 2 end: [1.03902664248502;-8.17043921019233;1.1044621473958]
Robot Arm 1 togoal: 11.1973615499254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.636916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [-0.111261928818144;-4.69224259562568;-0.0635558598027102]
Robot Arm 1 end: [-4.72911496675068;-0.427481923317369;1.13478084928421]
Robot Arm 1 togoal: 18.0507166671368
Mobile Platform 2 Pose: [-52.2584903601045;1.63396878638809;-8.33383725959338]
Robot Arm 2 end: [1.03714401483275;-7.87419519571109;0.945739872125785]
Robot Arm 1 togoal: 11.2547109374172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.854426e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [-0.121415103578972;-4.68629452539535;-0.0593671242008268]
Robot Arm 1 end: [-4.55145759490027;-0.387059008366613;1.1636059550041]
Robot Arm 1 togoal: 17.8819362341685
Mobile Platform 2 Pose: [-52.0207667559473;1.41986228554923;-8.3388238881393]
Robot Arm 2 end: [0.835199542880016;-7.95140288957541;1.10796783840801]
Robot Arm 1 togoal: 11.0348944182252

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.014099e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [-0.183867333308155;-4.64701637753637;-0.0515777937251123]
Robot Arm 1 end: [-4.34762538064108;-0.299078674576885;1.20534542938902]
Robot Arm 1 togoal: 17.6642297713851
Mobile Platform 2 Pose: [-51.7581773487456;1.16988592264756;-8.33538599172032]
Robot Arm 2 end: [0.618052998839819;-7.97097695120731;1.17586443590541]
Robot Arm 1 togoal: 10.8156877995025

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.459007e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [-0.273157755627049;-4.58441005892791;-0.0350788246367504]
Robot Arm 1 end: [-4.14799899654756;-0.210633675007361;1.2056341999031]
Robot Arm 1 togoal: 17.4505693671976
Mobile Platform 2 Pose: [-51.4257953192401;0.890779246128495;-8.34252176679178]
Robot Arm 2 end: [0.643259152995379;-8.17437373872202;1.38731693977248]
Robot Arm 1 togoal: 10.7995253508791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.687512e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [-0.344917532720483;-4.51932591879695;-0.0129954871361823]
Robot Arm 1 end: [-3.97265320390293;-0.194282637976099;1.16399081213386]
Robot Arm 1 togoal: 17.2998812602775
Mobile Platform 2 Pose: [-52.0144607968445;1.00887717341513;-8.31038652131429]
Robot Arm 2 end: [0.628831388139794;-8.25057582005723;1.38271504849577]
Robot Arm 1 togoal: 10.7727278052816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.298267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [-0.380112035146519;-4.4673087119841;0.00257793030297513]
Robot Arm 1 end: [-3.80199068670496;-0.116367619974324;1.09396169853053]
Robot Arm 1 togoal: 17.1177744595867
Mobile Platform 2 Pose: [-52.6455696314208;1.16021085048285;-8.29621009243084]
Robot Arm 2 end: [0.665460148479572;-8.20790974975658;1.3327059642767]
Robot Arm 1 togoal: 10.8166114050433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.010729e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [-0.271833626141195;-4.50577026263175;0.0164259916017659]
Robot Arm 1 end: [-3.81355026861061;-0.0462258671346646;1.10862255614055]
Robot Arm 1 togoal: 17.0853996252099
Mobile Platform 2 Pose: [-52.9328745709573;0.983692590164866;-8.33614166072414]
Robot Arm 2 end: [0.468739386460054;-7.87355639943945;1.16228748757528]
Robot Arm 1 togoal: 10.6885425290147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.831307e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.106589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [-0.177982202358346;-4.54606972866731;0.0262495884647905]
Robot Arm 1 end: [-3.92817517910581;-0.0266925745324987;1.21642328121026]
Robot Arm 1 togoal: 17.1645388747171
Mobile Platform 2 Pose: [-52.8625800174844;0.916977468762817;-8.33196202494772]
Robot Arm 2 end: [0.456495723175817;-8.06418761359252;1.33463368218308]
Robot Arm 1 togoal: 10.6358076707216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.922797e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.678855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [-0.068374635169161;-4.59524349295829;0.0376964822492177]
Robot Arm 1 end: [-4.11260019390355;-0.101966588326847;1.3129257672844]
Robot Arm 1 togoal: 17.3568000527447
Mobile Platform 2 Pose: [-52.9843608973873;0.67146239381346;-8.28481414925523]
Robot Arm 2 end: [0.105450303140829;-8.03533215361622;1.25211263258634]
Robot Arm 1 togoal: 10.298171973327

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.824973e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.415738e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [0.0740719476235675;-4.65712534358246;0.0570825274411791]
Robot Arm 1 end: [-4.21247404072348;-0.13599397733766;1.30794647812096]
Robot Arm 1 togoal: 17.4578974699146
Mobile Platform 2 Pose: [-53.0793975571322;0.357254434482162;-8.25632114685452]
Robot Arm 2 end: [-0.229614102468644;-7.89498922325264;1.17977348038934]
Robot Arm 1 togoal: 10.0003973168364

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928316e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.050470e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [0.166041147524658;-4.70416042029867;0.0664431256951452]
Robot Arm 1 end: [-4.32248029776267;-0.189465802611536;1.30687217315164]
Robot Arm 1 togoal: 17.5785239980102
Mobile Platform 2 Pose: [-53.3558402786169;0.263497736745004;-8.18960632363745]
Robot Arm 2 end: [-0.323935405411623;-7.72220977184678;1.08012530946514]
Robot Arm 1 togoal: 9.95032285179085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [0.256231228135014;-4.75178393868962;0.0759739821789689]
Robot Arm 1 end: [-4.43744900555602;-0.250306002018296;1.29309004693964]
Robot Arm 1 togoal: 17.7076438260359
Mobile Platform 2 Pose: [-53.1098178317387;0.0152699618930123;-8.15989148241808]
Robot Arm 2 end: [-0.611291574108401;-7.84008676685591;1.13090528707421]
Robot Arm 1 togoal: 9.64184862804698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959486e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.364231e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [0.351941742125714;-4.80308703770351;0.0871400979789896]
Robot Arm 1 end: [-4.5359605198991;-0.31585708581226;1.25440022684438]
Robot Arm 1 togoal: 17.8264447125616
Mobile Platform 2 Pose: [-52.7429656943691;-0.251088487123172;-8.17361005726566]
Robot Arm 2 end: [-0.688405237035648;-7.83871640476183;1.28328460303416]
Robot Arm 1 togoal: 9.56208410435013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.607248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [0.449811203325603;-4.85628934001914;0.0981129651566263]
Robot Arm 1 end: [-4.65534185451519;-0.386731920246975;1.21293468223026]
Robot Arm 1 togoal: 17.9654710234882
Mobile Platform 2 Pose: [-52.5563322634677;-0.736590322471451;-8.14999082000588]
Robot Arm 2 end: [-0.797315837008604;-8.13443473682711;1.44939569936469]
Robot Arm 1 togoal: 9.39014678963005

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.405277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [0.555890182303804;-4.91414820830982;0.110748831104503]
Robot Arm 1 end: [-4.76146702816856;-0.464806477713936;1.14433025998464]
Robot Arm 1 togoal: 18.0984795917435
Mobile Platform 2 Pose: [-53.1889278250565;-0.46776276134247;-8.08647859974042]
Robot Arm 2 end: [-0.5843889078458;-8.04050342376978;1.47183936324893]
Robot Arm 1 togoal: 9.6174724246634

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966163e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.047199e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [0.672071730532373;-4.97756692775329;0.124048883382538]
Robot Arm 1 end: [-4.81960245367533;-0.529652190198291;1.05225437537456]
Robot Arm 1 togoal: 18.185541360986
Mobile Platform 2 Pose: [-53.9019228382585;-0.269481918134005;-8.0489943319985]
Robot Arm 2 end: [-0.587378089593198;-7.71987178304257;1.37404284535618]
Robot Arm 1 togoal: 9.68597090225532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963260e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.063774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [0.79035471184379;-5.04224025463937;0.135379689561289]
Robot Arm 1 end: [-4.84479035399644;-0.568716078718795;0.958028806984308]
Robot Arm 1 togoal: 18.2313602556376
Mobile Platform 2 Pose: [-53.5978870245723;-0.519315928354045;-8.05810338098397]
Robot Arm 2 end: [-0.802595791022494;-7.80242826779164;1.42781618829256]
Robot Arm 1 togoal: 9.45675678677858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961592e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.748542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [0.900900314141841;-5.10304015912268;0.144170871522532]
Robot Arm 1 end: [-4.85788725085863;-0.585508428925256;0.880190567276148]
Robot Arm 1 togoal: 18.2543265826801
Mobile Platform 2 Pose: [-53.2688021468846;-0.768727050870515;-8.04045793115833]
Robot Arm 2 end: [-1.04893564372991;-7.88838370322041;1.43548015371115]
Robot Arm 1 togoal: 9.19716206984651

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967360e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.311468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [0.999807022364287;-5.15774809427331;0.151142564992788]
Robot Arm 1 end: [-4.75092538186384;-0.545423999722719;0.755895804033425]
Robot Arm 1 togoal: 18.1488608839764
Mobile Platform 2 Pose: [-53.4156648965567;-1.00479024244559;-8.00467945899425]
Robot Arm 2 end: [-1.38440518432758;-7.86442505460685;1.38481433660411]
Robot Arm 1 togoal: 8.87737015732401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979176e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.108314e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [1.08219798382043;-5.20594172259675;0.154246012871521]
Robot Arm 1 end: [-4.69758213443065;-0.493243078876822;0.752080797231955]
Robot Arm 1 togoal: 18.075350830651
Mobile Platform 2 Pose: [-53.7731118251326;-1.07238609017814;-7.98985227786233]
Robot Arm 2 end: [-1.48842241142622;-7.78530001602075;1.37050717019502]
Robot Arm 1 togoal: 8.79627746021986

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967061e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.363744e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [1.17826521360032;-5.26353035419879;0.156061373360741]
Robot Arm 1 end: [-4.64818807849797;-0.424324355677216;0.691053876410918]
Robot Arm 1 togoal: 17.9979099570888
Mobile Platform 2 Pose: [-53.5849843381609;-1.35575211943982;-8.03693114402891]
Robot Arm 2 end: [-1.38642619595708;-8.01404725003662;1.50109715629244]
Robot Arm 1 togoal: 8.83957341312176

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971527e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.643334e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [1.31332739385078;-5.34891826295622;0.153793924824475]
Robot Arm 1 end: [-4.62020363272422;-0.314086773435989;0.666399029645934]
Robot Arm 1 togoal: 17.9125956536405
Mobile Platform 2 Pose: [-53.7477479541027;-1.33845652038467;-7.98024146676407]
Robot Arm 2 end: [-1.10324302867435;-8.03399556377713;1.43481301648089]
Robot Arm 1 togoal: 9.11179928697991

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951030e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.960685e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [1.48055623047047;-5.45606468758473;0.14711442963834]
Robot Arm 1 end: [-4.6340079087812;-0.157290085650606;0.611365854319718]
Robot Arm 1 togoal: 17.8368204464056
Mobile Platform 2 Pose: [-53.8943408127786;-1.34714588538968;-7.91855412615953]
Robot Arm 2 end: [-1.0800365466546;-8.05335696631809;1.44459224177254]
Robot Arm 1 togoal: 9.13022481952757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796697e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.326279e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [1.58835767109955;-5.70768859901916;-0.010418981361109]
Robot Arm 1 end: [-4.8477188942389;-0.139975077623987;0.51842065192008]
Robot Arm 1 togoal: 18.0077487845197
Mobile Platform 2 Pose: [-54.2196327403119;-1.47098774800976;-7.83914028150353]
Robot Arm 2 end: [-1.28794782908365;-7.95221585371167;1.41987093585689]
Robot Arm 1 togoal: 8.95005586720958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.598945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [1.44740357899207;-5.67887111330521;-0.0646055715320513]
Robot Arm 1 end: [-4.83918303704987;-0.294834337648275;0.643654496587833]
Robot Arm 1 togoal: 18.0818887945659
Mobile Platform 2 Pose: [-54.4597519725099;-1.470371396315;-7.78862114433817]
Robot Arm 2 end: [-1.17584300965771;-8.0585956532927;1.39767516607608]
Robot Arm 1 togoal: 9.03603931168314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.819526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [1.2783893141081;-5.61127779547489;-0.0889141170441355]
Robot Arm 1 end: [-4.76487570205436;-0.299976626204228;0.72699777799257]
Robot Arm 1 togoal: 18.020030869175
Mobile Platform 2 Pose: [-54.9857601915159;-1.45455625283269;-7.6149318941157]
Robot Arm 2 end: [-1.22587841370748;-7.78384351782777;1.40805223546604]
Robot Arm 1 togoal: 9.05037658675216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.223003e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [0.920764346631552;-5.42374223543044;-0.104711006343975]
Robot Arm 1 end: [-4.6742022371875;-0.406036755172825;0.939327449703183]
Robot Arm 1 togoal: 17.9987820477847
Mobile Platform 2 Pose: [-55.4050161829843;-1.33092502357483;-7.617214664619]
Robot Arm 2 end: [-1.07881832406918;-7.93260346643192;1.4012788682788]
Robot Arm 1 togoal: 9.15838108354057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.833322e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.656203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [1.16227689304417;-5.54500452215495;-0.0889333718196492]
Robot Arm 1 end: [-4.74636547321869;-0.284223671638447;0.943874547363522]
Robot Arm 1 togoal: 17.9875964204811
Mobile Platform 2 Pose: [-56.0644372264671;-1.14271667602055;-7.59659643214258]
Robot Arm 2 end: [-0.81690089968354;-7.99642041195253;1.34542010273554]
Robot Arm 1 togoal: 9.40076957233805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.936063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.447039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [1.23078680285845;-5.56839893619056;-0.0761960546060314]
Robot Arm 1 end: [-4.79422033039684;-0.168054879288313;1.04814360189537]
Robot Arm 1 togoal: 17.9577807199491
Mobile Platform 2 Pose: [-56.620028233054;-1.02646447772827;-7.57441204787906]
Robot Arm 2 end: [-0.764614070308598;-8.12181085782725;1.27401248788315]
Robot Arm 1 togoal: 9.42766941270531

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000190e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.157458e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [1.3124994400066;-5.60276254867912;-0.0667684812593043]
Robot Arm 1 end: [-4.89319335874705;-0.0952189499002712;1.15083949155732]
Robot Arm 1 togoal: 17.9960460343227
Mobile Platform 2 Pose: [-56.9305895554697;-0.937186198143215;-7.55481473581287]
Robot Arm 2 end: [-0.859716851929136;-8.15247161360791;1.25442544612927]
Robot Arm 1 togoal: 9.32894416095434

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.895342e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [1.41078720921827;-5.64581211279897;-0.0583096395288925]
Robot Arm 1 end: [-5.04620760784873;-0.127171832234817;1.24365528911961]
Robot Arm 1 togoal: 18.139043326117
Mobile Platform 2 Pose: [-57.4123041843988;-0.821001706365383;-7.46715725267587]
Robot Arm 2 end: [-0.665294848472157;-8.12278610763372;1.12662189256157]
Robot Arm 1 togoal: 9.52975269119194

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.048648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [1.56044135893911;-5.69365051106555;-0.0507814839970488]
Robot Arm 1 end: [-5.22644597447439;-0.028241706001041;1.35061965102218]
Robot Arm 1 togoal: 18.2329185318995
Mobile Platform 2 Pose: [-57.6650740561431;-0.738668499559289;-7.44733965938547]
Robot Arm 2 end: [-0.756748815046059;-8.1722062715668;1.10721599178934]
Robot Arm 1 togoal: 9.43131695932774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004900e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [1.77443026383182;-5.73163189677529;-0.0396923709573003]
Robot Arm 1 end: [-5.44484489680174;0.0376703443795219;1.42624725418626]
Robot Arm 1 togoal: 18.3793423205608
Mobile Platform 2 Pose: [-58.4634488692243;-0.477573329392826;-7.27280326482502]
Robot Arm 2 end: [-0.6500997190744;-7.89933744740052;1.2118039831478]
Robot Arm 1 togoal: 9.5879622756863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981318e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.726673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [1.91435399713594;-5.63972504724368;-0.028894143403458]
Robot Arm 1 end: [-5.50083379626042;0.0633807095050283;1.44888216695069]
Robot Arm 1 togoal: 18.4124265724366
Mobile Platform 2 Pose: [-59.1770281648205;-0.212027665823464;-7.14301591912803]
Robot Arm 2 end: [-0.452288092211613;-7.6421447523104;1.28505397442386]
Robot Arm 1 togoal: 9.83737539032473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.868851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.763594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [1.8272491687643;-5.47376519664802;-0.0370782854938211]
Robot Arm 1 end: [-5.17185210781499;0.105235379587961;1.414871287721]
Robot Arm 1 togoal: 18.113606117456
Mobile Platform 2 Pose: [-59.7655862447645;0.0260727534689369;-7.04710411911784]
Robot Arm 2 end: [-0.0547407504223154;-7.63436149892173;1.27059609733166]
Robot Arm 1 togoal: 10.2258070182048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.420013e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [1.72181007585356;-5.27908788797399;-0.0424289598393102]
Robot Arm 1 end: [-4.7920934800821;0.0563885072767094;1.33041870622222]
Robot Arm 1 togoal: 17.8246391343905
Mobile Platform 2 Pose: [-60.443362010425;0.26732815272723;-6.98154508962719]
Robot Arm 2 end: [0.109137836546432;-7.46403639410572;1.33284243860696]
Robot Arm 1 togoal: 10.4240674631654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [1.90447419069826;-5.23896576645485;-0.0257974143071257]
Robot Arm 1 end: [-4.90689701605482;0.0796681033552458;1.40370404593033]
Robot Arm 1 togoal: 17.9064882634115
Mobile Platform 2 Pose: [-61.1206296398553;0.508221343611412;-6.88980943953758]
Robot Arm 2 end: [0.566274150875181;-7.29646627110506;1.39071267542574]
Robot Arm 1 togoal: 10.9074386930054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.874125e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [2.05209016336098;-5.16710827034682;-0.00720745989094775]
Robot Arm 1 end: [-4.96677234432482;0.143360399809702;1.42180647700924]
Robot Arm 1 togoal: 17.9211455540624
Mobile Platform 2 Pose: [-61.4294592802112;0.378795618685487;-6.81490033370023]
Robot Arm 2 end: [0.575482603053654;-7.26408968606989;1.33147786911082]
Robot Arm 1 togoal: 10.9252897522883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.474875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [1.96608098445595;-5.01772357047015;-0.0154819377332024]
Robot Arm 1 end: [-4.65658279355248;0.185014736711581;1.37076489870752]
Robot Arm 1 togoal: 17.6400659150834
Mobile Platform 2 Pose: [-61.7528143730233;0.447921537068044;-6.82504295309196]
Robot Arm 2 end: [0.492675044784822;-7.28584024846651;1.35953004072438]
Robot Arm 1 togoal: 10.8392326896346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.794983e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [1.86248994830075;-4.81542871706103;-0.00544354545100293]
Robot Arm 1 end: [-4.26299030510944;0.143506813130823;1.26723151912977]
Robot Arm 1 togoal: 17.3391968910999
Mobile Platform 2 Pose: [-62.3374130478088;0.52978257555132;-6.75696899185197]
Robot Arm 2 end: [0.764347940938553;-7.21603848689151;1.32717707218401]
Robot Arm 1 togoal: 11.120215628638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [2.07522275467955;-4.83160069437124;0.0264242303685713]
Robot Arm 1 end: [-4.46918107992072;0.227266756519637;1.36782261128696]
Robot Arm 1 togoal: 17.4610131288014
Mobile Platform 2 Pose: [-62.7724883517009;0.635762409668919;-6.77659474795287]
Robot Arm 2 end: [0.746345214291188;-7.19191861597028;1.37584569852795]
Robot Arm 1 togoal: 11.1081198361959

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006931e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [2.00442649591135;-4.63578775782441;0.0421239191944354]
Robot Arm 1 end: [-4.08390170193109;0.189337862809343;1.26221213936389]
Robot Arm 1 togoal: 17.1660231235012
Mobile Platform 2 Pose: [-63.2665920074742;0.677491943694322;-6.7988212603901]
Robot Arm 2 end: [0.864215510080974;-7.17435096729964;1.37859330818871]
Robot Arm 1 togoal: 11.2265645133031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [2.21152893694914;-4.648526782337;0.0716966532914387]
Robot Arm 1 end: [-4.28401509559444;0.273312891232359;1.36077574085025]
Robot Arm 1 togoal: 17.2819906601137
Mobile Platform 2 Pose: [-63.6541759718577;0.758890477627204;-6.88630003089223]
Robot Arm 2 end: [0.888239567710811;-7.3823558639288;1.3330530450236]
Robot Arm 1 togoal: 11.2000511501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [2.12026483405693;-4.45630907298216;0.0750883424097146]
Robot Arm 1 end: [-3.90013232293387;0.224342810541924;1.25460984978761]
Robot Arm 1 togoal: 16.9955322303541
Mobile Platform 2 Pose: [-63.8575848587545;0.843853279909958;-6.89250539124002]
Robot Arm 2 end: [0.826528529169809;-7.39669396823836;1.35139827742865]
Robot Arm 1 togoal: 11.1364126644191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.995565e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [2.32520737480038;-4.46579235863873;0.103614014231407]
Robot Arm 1 end: [-4.07812262701307;0.324286202945276;1.33964984345205]
Robot Arm 1 togoal: 17.0834967750462
Mobile Platform 2 Pose: [-64.2030606880558;0.976997809774767;-6.89795915799506]
Robot Arm 2 end: [0.862626225093563;-7.39010254613882;1.35034872116302]
Robot Arm 1 togoal: 11.1730629231527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.986019e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [2.22570534911166;-4.26708449531754;0.109473847801097]
Robot Arm 1 end: [-3.68761585777133;0.27155062279114;1.22648142354432]
Robot Arm 1 togoal: 16.7952462425148
Mobile Platform 2 Pose: [-64.5583878480871;1.11483019612672;-6.90146940130363]
Robot Arm 2 end: [0.93235506373367;-7.3482107563648;1.36657755017244]
Robot Arm 1 togoal: 11.2504319794936

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.662773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [2.43416396763277;-4.28387843841909;0.139241204423432]
Robot Arm 1 end: [-3.86296106801397;0.388197466759178;1.30756105984284]
Robot Arm 1 togoal: 16.870506622282
Mobile Platform 2 Pose: [-65.0319704663734;1.29673650266593;-6.91396674540022]
Robot Arm 2 end: [1.06350621179028;-7.27745977024822;1.3993685686891]
Robot Arm 1 togoal: 11.3942173582051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.587383e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [2.30568245912765;-4.07368515058714;0.147259993420744]
Robot Arm 1 end: [-3.4600966275419;0.329084088476815;1.18449081323913]
Robot Arm 1 togoal: 16.5775165276133
Mobile Platform 2 Pose: [-65.6640772049144;1.45251304139034;-6.91619559300438]
Robot Arm 2 end: [1.35515821771501;-7.22693715204013;1.45093502208228]
Robot Arm 1 togoal: 11.6890720251527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.094682e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.038057e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [2.5198656222932;-4.10164746340619;0.179825018632962]
Robot Arm 1 end: [-3.65019324547323;0.455724498921599;1.27628405669798]
Robot Arm 1 togoal: 16.6577566428422
Mobile Platform 2 Pose: [-66.0555231342801;1.43782912600163;-6.99910469838628]
Robot Arm 2 end: [1.27669596324414;-7.36996989066137;1.41218550717227]
Robot Arm 1 togoal: 11.5798432984788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.484348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.319456e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [2.36443274267226;-3.87992314011492;0.1924764336593]
Robot Arm 1 end: [-3.23637422654392;0.391694387661169;1.14218749853399]
Robot Arm 1 togoal: 16.3604595997938
Mobile Platform 2 Pose: [-66.4438315394544;1.57384858183426;-7.00156975318508]
Robot Arm 2 end: [1.34727380907348;-7.30909111460091;1.42876317702389]
Robot Arm 1 togoal: 11.6623377227346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.673237e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.142961e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [2.58120269401916;-3.9152642464887;0.227366806776815]
Robot Arm 1 end: [-3.38644083897127;0.568071717394918;1.17655905555425]
Robot Arm 1 togoal: 16.3791547104145
Mobile Platform 2 Pose: [-67.0652388165855;1.75189159990375;-7.04037589704868]
Robot Arm 2 end: [1.63526816457951;-7.30310363236244;1.46841894179456]
Robot Arm 1 togoal: 11.9438469534374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999227e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.924017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [2.47748426599121;-3.67040456997562;0.277802822886349]
Robot Arm 1 end: [-2.99092376960789;0.552708399215299;1.04702977373066]
Robot Arm 1 togoal: 16.0698496182575
Mobile Platform 2 Pose: [-67.469673139332;1.78154931318216;-7.13692754016048]
Robot Arm 2 end: [1.54965516014718;-7.41995172191459;1.42833167695812]
Robot Arm 1 togoal: 11.8346850762351

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.076189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.004585e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [2.73019616616274;-3.69865582476535;0.327587246084266]
Robot Arm 1 end: [-3.21252132065665;0.701326081615422;1.18564179277713]
Robot Arm 1 togoal: 16.1600903901062
Mobile Platform 2 Pose: [-67.9444417132157;1.94590901614039;-7.13772179150956]
Robot Arm 2 end: [1.73220681957524;-7.30999964633671;1.47222473634967]
Robot Arm 1 togoal: 12.0367419921813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.669833e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.195207e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [2.50235867703421;-3.48442781416305;0.342018684779426]
Robot Arm 1 end: [-2.80915561195288;0.616575311454257;1.05426037121903]
Robot Arm 1 togoal: 15.8852438526422
Mobile Platform 2 Pose: [-68.1157853243874;1.70450195492863;-7.12319194575988]
Robot Arm 2 end: [1.57920659131586;-7.28991549222421;1.48122982539618]
Robot Arm 1 togoal: 11.8921892429631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009296e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.468327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [2.76086381394977;-3.50014637623503;0.391798576489865]
Robot Arm 1 end: [-3.01383184455489;0.765936773880222;1.18523470798215]
Robot Arm 1 togoal: 15.9605915076369
Mobile Platform 2 Pose: [-68.0006351856276;1.4858593687223;-7.00910493125325]
Robot Arm 2 end: [1.23083233208846;-7.22169984383795;1.44799632829692]
Robot Arm 1 togoal: 11.5696100265003

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.895961e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.116746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [2.53853660787391;-3.28728876072168;0.407030771072619]
Robot Arm 1 end: [-2.61213902876832;0.681979417698067;1.05419203525579]
Robot Arm 1 togoal: 15.687876469718
Mobile Platform 2 Pose: [-68.5164729473252;1.53926480379812;-7.01235940931116]
Robot Arm 2 end: [1.42281871889061;-7.21444928435143;1.4760843084802]
Robot Arm 1 togoal: 11.7576399712456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024119e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.078102e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [2.79773651980064;-3.30388570723137;0.456722761135525]
Robot Arm 1 end: [-2.81810981645972;0.83130139744981;1.18541115663744]
Robot Arm 1 togoal: 15.7632989314742
Mobile Platform 2 Pose: [-68.5275572501654;1.30169451079258;-6.88494727828648]
Robot Arm 2 end: [1.13530944180843;-7.18825270410401;1.41703036429964]
Robot Arm 1 togoal: 11.4852883684283

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.927968e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.119463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [2.57827648421233;-3.09482231933422;0.470890792386904]
Robot Arm 1 end: [-2.42063466082363;0.746242004177899;1.05549509776901]
Robot Arm 1 togoal: 15.4958348351064
Mobile Platform 2 Pose: [-68.9241295533843;1.36361325526714;-6.91203965892533]
Robot Arm 2 end: [1.23134997074314;-7.29210952204749;1.40605789865001]
Robot Arm 1 togoal: 11.5537485126067

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.031701e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.992896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [2.83673455719123;-3.10765453459063;0.520675024517873]
Robot Arm 1 end: [-2.62225229869709;0.895668896505302;1.18546077679912]
Robot Arm 1 togoal: 15.5667172038401
Mobile Platform 2 Pose: [-69.4347312870049;1.48055112609016;-6.9783690301412]
Robot Arm 2 end: [1.42310393304303;-7.46228524855334;1.36285704044001]
Robot Arm 1 togoal: 11.7026624671684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945890e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.750813e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [2.61792077701106;-2.90013603271867;0.534358604500707]
Robot Arm 1 end: [-2.22644609381697;0.810149232450384;1.05601136242505]
Robot Arm 1 togoal: 15.3021421914684
Mobile Platform 2 Pose: [-69.7866727984445;1.61358435885549;-6.9932115686189]
Robot Arm 2 end: [1.41884022577366;-7.53008779445515;1.3037201928649]
Robot Arm 1 togoal: 11.6849299920639

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.039573e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.845869e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [2.87575093085223;-2.91142420168098;0.584128286458581]
Robot Arm 1 end: [-2.42631717936189;0.9594866064175;1.18547396489541]
Robot Arm 1 togoal: 15.3706479623301
Mobile Platform 2 Pose: [-70.1326137211059;1.74901992768167;-6.98606556253557]
Robot Arm 2 end: [1.45007123894528;-7.45086477503761;1.31124033645523]
Robot Arm 1 togoal: 11.7322726224938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986343e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000989e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.34228866715839;9.99051096091294;9.8185121341582]
Robot Arm 2 end: [9.76841269050066;9.82919946172778;1.48915541732611]
Robot Arm 1 togoal: 27.9997911719423

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.14405677446962;9.85642097223373;9.81439923197141]
Robot Arm 2 end: [9.54282893614189;9.88359111560618;1.45995843154117]
Robot Arm 1 togoal: 27.8797970680024

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.857325e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-1.96886528268769;9.73091570178791;9.82707841677725]
Robot Arm 2 end: [9.22142467370475;9.94848744386392;1.34346456667242]
Robot Arm 1 togoal: 27.7026503536212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.924378e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.85312447372185;9.61905180264841;9.85290473658816]
Robot Arm 2 end: [9.01888391407314;10.0115768364022;1.26321472145547]
Robot Arm 1 togoal: 27.608832030096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.895606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.73353812401914;9.49250238877296;9.88392432434691]
Robot Arm 2 end: [8.82625451858346;10.0537686239982;1.19008627759711]
Robot Arm 1 togoal: 27.5080169971413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.882090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.57653514737696;9.36263896533314;9.89932657021198]
Robot Arm 2 end: [8.67096192465603;10.1091757371533;1.12825575795911]
Robot Arm 1 togoal: 27.4433601452032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.781497e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.44491296319782;9.29735745751766;9.89323179868271]
Robot Arm 2 end: [8.52868882399367;9.96867943783034;1.0477068788008]
Robot Arm 1 togoal: 27.2448981353535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.900084e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.36432516711132;9.251257509536;9.8881549525086]
Robot Arm 2 end: [8.50728952723371;9.82257968364439;1.09806070082868]
Robot Arm 1 togoal: 27.1225608785393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.931385e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.29293405740446;9.2104546643712;9.88101022300813]
Robot Arm 2 end: [8.52200285702602;9.70267406402077;1.1436039053707]
Robot Arm 1 togoal: 27.0444519804147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.2230156213611;9.17155538621862;9.87149634550147]
Robot Arm 2 end: [8.51988678514224;9.60619146273948;1.13428425960931]
Robot Arm 1 togoal: 26.9729215931821

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.15149599315538;9.13257897429823;9.8589288256801]
Robot Arm 2 end: [8.50065031355435;9.48524417595179;1.07995382010465]
Robot Arm 1 togoal: 26.8727245428089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.0485496047847;9.0796182902428;9.84666849910677]
Robot Arm 2 end: [8.51045968992364;9.32233598490308;0.99910565788003]
Robot Arm 1 togoal: 26.7630745533862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-0.912695289188472;9.01128069459117;9.83087719933467]
Robot Arm 2 end: [8.56286815077939;9.16772227077657;0.907601853425063]
Robot Arm 1 togoal: 26.6900338763192

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.750151364886102;8.92833108132548;9.80599877741873]
Robot Arm 2 end: [8.64131466847617;9.04655393967866;0.788870890512525]
Robot Arm 1 togoal: 26.6609422672792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987969e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.529886022886471;8.81101877287321;9.75937820879345]
Robot Arm 2 end: [8.6378237647111;8.9793878012976;0.742302529108513]
Robot Arm 1 togoal: 26.6118771556545

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965428e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.230512011803202;8.63288818406978;9.67879625156232]
Robot Arm 2 end: [8.56077259867972;8.91662308641202;0.782757491187587]
Robot Arm 1 togoal: 26.5119955954865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991789e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [0.0734747258538684;8.40995028722634;9.56566178156443]
Robot Arm 2 end: [8.54060244835253;8.81407969029645;0.700539499960071]
Robot Arm 1 togoal: 26.4271960141973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [0.248456074763555;8.2304594751095;9.3971650960415]
Robot Arm 2 end: [8.42985558713016;8.66515978001384;0.825967243218021]
Robot Arm 1 togoal: 26.2398711336188

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002012e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [0.441345244246249;8.07709583581389;9.21820353003385]
Robot Arm 2 end: [8.43231015948154;8.62890005121518;0.974483074770182]
Robot Arm 1 togoal: 26.2122997022973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [0.258957190060973;8.06623401356211;9.04838958766502]
Robot Arm 2 end: [8.31731198027607;8.37411998861782;0.951852496467507]
Robot Arm 1 togoal: 25.9510333593803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952287e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [0.266268853048988;7.98635854190925;8.84757292700031]
Robot Arm 2 end: [8.15728944004195;8.13468651905695;0.919895214796663]
Robot Arm 1 togoal: 25.6693463476716
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [0.590144604391404;7.78561748616802;8.72311993511497]
Robot Arm 2 end: [8.00601141891127;8.10607846551058;1.03610062651119]
Robot Arm 1 togoal: 25.539805246313
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [0.877390387426225;7.56887570657826;8.59852859921921]
Robot Arm 2 end: [7.8955194889181;8.1455916645595;1.16207510207117]
Robot Arm 1 togoal: 25.4880548829747
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [1.05186539050086;7.36499134539683;8.45385252218558]
Robot Arm 2 end: [7.65159802626138;8.1078224773982;1.2811709905481]
Robot Arm 1 togoal: 25.2889237751917
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [1.18106922109429;7.11852276314021;8.41198083160727]
Robot Arm 2 end: [7.30584072362084;8.12298447238077;1.35781974623766]
Robot Arm 1 togoal: 25.0591563531717
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [1.20097595607279;6.88360449412634;8.32646237229279]
Robot Arm 2 end: [6.97792925518702;8.12667210072058;1.42439445487991]
Robot Arm 1 togoal: 24.8361763537088
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [1.42548101166134;6.6366747485514;8.28740214499474]
Robot Arm 2 end: [6.66572668353901;8.20544672386252;1.47570655307942]
Robot Arm 1 togoal: 24.6817095802166
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [1.7306944486666;6.39259424493282;8.23332157774786]
Robot Arm 2 end: [6.31907240798174;8.24238701105801;1.48825895279677]
Robot Arm 1 togoal: 24.476476055477
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [2.02533496620911;6.17290643753792;8.18293982590704]
Robot Arm 2 end: [5.98743283272755;8.33217880706737;1.45585632089723]
Robot Arm 1 togoal: 24.3242889329615
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [2.26552758496491;5.99994864113312;8.15356577508242]
Robot Arm 2 end: [5.76140986636591;8.48436243855785;1.37153183445914]
Robot Arm 1 togoal: 24.2923040584473
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [2.42223598981532;5.90053950775918;8.14782325153891]
Robot Arm 2 end: [5.59240963914047;8.55971427556396;1.31401381378261]
Robot Arm 1 togoal: 24.2410617677242
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [2.53431672118234;5.83186671595048;8.1480972501881]
Robot Arm 2 end: [5.47072288521933;8.5908662794571;1.2756833101446]
Robot Arm 1 togoal: 24.1872643325
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [2.6216846771549;5.77956895702612;8.14942371232527]
Robot Arm 2 end: [5.37328887648095;8.58753992577228;1.25972503484713]
Robot Arm 1 togoal: 24.1226639486464
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [2.69900257225917;5.7340595296049;8.1507372085814]
Robot Arm 2 end: [5.28672286978492;8.56957309297415;1.25172441849058]
Robot Arm 1 togoal: 24.0538032396482
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [2.77329569716337;5.69096320665913;8.15178952792245]
Robot Arm 2 end: [5.20842363150508;8.54883917427019;1.24431270814784]
Robot Arm 1 togoal: 23.9881666645397
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [2.84818155790778;5.64895448205396;8.15176883446788]
Robot Arm 2 end: [5.13956805503423;8.53173612990796;1.23515857963942]
Robot Arm 1 togoal: 23.9314326817084
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [2.92472251280373;5.60871971278828;8.14940678475887]
Robot Arm 2 end: [5.08240699958475;8.51819601708504;1.22535909758595]
Robot Arm 1 togoal: 23.8849322412999
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [3.00081146543284;5.57178073739996;8.14411041976658]
Robot Arm 2 end: [5.03628102996218;8.49978014447919;1.21715973368411]
Robot Arm 1 togoal: 23.8416428023218
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [3.07451368587744;5.53785186148261;8.13686890790347]
Robot Arm 2 end: [4.9977466769085;8.47001438796544;1.20991878805139]
Robot Arm 1 togoal: 23.7943398394828
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [3.14534682853878;5.50549908671979;8.12934460149571]
Robot Arm 2 end: [4.96288646311489;8.42759834259602;1.20142081814449]
Robot Arm 1 togoal: 23.7395552367265
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [3.21311619785882;5.47395471549227;8.12259244821249]
Robot Arm 2 end: [4.93474463712606;8.38668949820053;1.19007015753586]
Robot Arm 1 togoal: 23.6902187123723
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [3.27624716181771;5.44502105191235;8.11522223881443]
Robot Arm 2 end: [4.94118550679352;8.40187140773559;1.17016938951764]
Robot Arm 1 togoal: 23.7063488916473
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [3.35524718321283;5.40887179113815;8.10099843080107]
Robot Arm 2 end: [5.00348756870717;8.39909241015757;1.19673162855949]
Robot Arm 1 togoal: 23.7431335488512
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [3.45934401084102;5.35551483554634;8.08349018063409]
Robot Arm 2 end: [5.05067966385391;8.38633986820347;1.21059034423336]
Robot Arm 1 togoal: 23.7629292438333
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [3.58015554458652;5.28829223241422;8.067657623473]
Robot Arm 2 end: [5.06377990379154;8.36184964195991;1.20791374543146]
Robot Arm 1 togoal: 23.7523264179873
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [3.70316960236691;5.21740568449594;8.05657496723162]
Robot Arm 2 end: [5.05146642801766;8.32906440973403;1.19388512139175]
Robot Arm 1 togoal: 23.7193644078784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.473424e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307231667504;-6.24962959885203;-7.10439671995214]
Robot Arm 1 end: [-6.13860520509773;-6.93244387864659;1.24046676568927]
Robot Arm 1 togoal: 23.3931813262984
Mobile Platform 2 Pose: [3.81924802344052;5.14981903074051;8.04781681070369]
Robot Arm 2 end: [5.03943311285586;8.28510812416292;1.17520875621455]
Robot Arm 1 togoal: 23.6780341044748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.667506e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407597063525;-6.21793448810926;-7.1033776574255]
Robot Arm 1 end: [-6.10318024585557;-6.88095808800451;1.17439343517582]
Robot Arm 1 togoal: 23.3323658489716
Mobile Platform 2 Pose: [3.92498829784995;5.08745432978804;8.04037937482681]
Robot Arm 2 end: [5.01700547670273;8.21690847561392;1.15425278988101]
Robot Arm 1 togoal: 23.6114529712245

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.953108e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859261074875;-6.18978070787067;-7.10219157273074]
Robot Arm 1 end: [-6.07851976498904;-6.82867780649832;1.10278359149526]
Robot Arm 1 togoal: 23.2787048504081
Mobile Platform 2 Pose: [4.02345670258216;5.02893525482683;8.03418707081726]
Robot Arm 2 end: [4.97003020429196;8.12358592852221;1.12480063032715]
Robot Arm 1 togoal: 23.510064763128

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000057e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70845331756692;-6.16355514088787;-7.10108006162275]
Robot Arm 1 end: [-6.05859523708747;-6.77642517246908;1.02303733350297]
Robot Arm 1 togoal: 23.2287513520053
Mobile Platform 2 Pose: [4.1286156437619;4.9663937877965;8.0297217640795]
Robot Arm 2 end: [4.92587799935245;8.023989086242;1.1184468735972]
Robot Arm 1 togoal: 23.4052999479199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000260e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.935846e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74535974750668;-6.13815955461115;-7.1000057128429]
Robot Arm 1 end: [-6.04159009204588;-6.72130618089952;0.930971265578765]
Robot Arm 1 togoal: 23.1793620918276
Mobile Platform 2 Pose: [4.24925778003279;4.89332440639812;8.02539159339911]
Robot Arm 2 end: [4.87568457407781;7.89988359959411;1.1197225509896]
Robot Arm 1 togoal: 23.2777328796762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991271e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880981e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78013099976555;-6.11348723969434;-7.0988318120806]
Robot Arm 1 end: [-6.028969438596;-6.65925637538658;0.819170863333191]
Robot Arm 1 togoal: 23.1290131302954
Mobile Platform 2 Pose: [4.34634085791128;4.82957880882015;8.01368537521811]
Robot Arm 2 end: [4.81144686061969;7.72277014155313;1.10026166740359]
Robot Arm 1 togoal: 23.1009212011081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977963e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945144e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80724207213463;-6.09291235394148;-7.09790607083278]
Robot Arm 1 end: [-5.99722534480655;-6.60139009429755;0.729715803974582]
Robot Arm 1 togoal: 23.0682357902472
Mobile Platform 2 Pose: [4.38314244717213;4.79777754091169;7.99382266480985]
Robot Arm 2 end: [4.72050805476688;7.54934001511816;1.02080581251955]
Robot Arm 1 togoal: 22.9111956602416

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005471e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82883333513147;-6.07529467762279;-7.0984296697567]
Robot Arm 1 end: [-5.97781043168812;-6.5635510412865;0.646586790374218]
Robot Arm 1 togoal: 23.0305503136472
Mobile Platform 2 Pose: [4.45248555769352;4.75403061296084;7.97323908008148]
Robot Arm 2 end: [4.62511208747815;7.52977740615789;1.07867102025211]
Robot Arm 1 togoal: 22.8338050794664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987489e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001315e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85467861791028;-6.05590617324849;-7.10073538590506]
Robot Arm 1 end: [-5.9485406278376;-6.53420928037476;0.572538536353823]
Robot Arm 1 togoal: 22.9920769795388
Mobile Platform 2 Pose: [4.57320192452969;4.68402847121995;7.95055363816932]
Robot Arm 2 end: [4.4969964252686;7.55834443781645;1.16616536780076]
Robot Arm 1 togoal: 22.7724454630472

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89488271238008;-6.02935346916335;-7.1042511653539]
Robot Arm 1 end: [-5.91354191959084;-6.51038204672492;0.511493091820798]
Robot Arm 1 togoal: 22.9532707027354
Mobile Platform 2 Pose: [4.74008061428681;4.58815172160304;7.92768303837337]
Robot Arm 2 end: [4.31302686308275;7.59718804370733;1.22091359635877]
Robot Arm 1 togoal: 22.6851012093527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94495090838422;-5.99854094113787;-7.10838710573599]
Robot Arm 1 end: [-5.88427782972397;-6.49016968242658;0.462506753168979]
Robot Arm 1 togoal: 22.9206541416398
Mobile Platform 2 Pose: [4.91299473249445;4.48811068335681;7.91192331444711]
Robot Arm 2 end: [4.15680616047269;7.66804286803303;1.27657238007168]
Robot Arm 1 togoal: 22.6414364454474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982177e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99690890024892;-5.96812988365884;-7.11341281883691]
Robot Arm 1 end: [-5.85823143450942;-6.47356146980332;0.424586215763296]
Robot Arm 1 togoal: 22.8924409995115
Mobile Platform 2 Pose: [5.07790544539476;4.39146298029182;7.90297031110182]
Robot Arm 2 end: [4.05340939737663;7.74124879476447;1.33011383047184]
Robot Arm 1 togoal: 22.6337505071016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967158e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.04668999259771;-5.94085502721957;-7.11957489216151]
Robot Arm 1 end: [-5.80811382841891;-6.46977726705346;0.388720375503669]
Robot Arm 1 togoal: 22.856773245291
Mobile Platform 2 Pose: [5.18795105520724;4.32573420235374;7.90234874016359]
Robot Arm 2 end: [3.93410292120939;7.71329949094161;1.31270434750915]
Robot Arm 1 togoal: 22.5380476118384

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.744364e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.07843260676481;-5.92506300535286;-7.12406624350709]
Robot Arm 1 end: [-5.76000329165577;-6.47634592781586;0.373742691064849]
Robot Arm 1 togoal: 22.8290139563442
Mobile Platform 2 Pose: [5.28461130386463;4.26694694582604;7.90360135171696]
Robot Arm 2 end: [3.78499306118475;7.67036485078961;1.24686344233406]
Robot Arm 1 togoal: 22.4130091355262

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.827080e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.11906077152963;-5.89828617950442;-7.12344898431193]
Robot Arm 1 end: [-5.76060503224125;-6.48355452985977;0.37963647713159]
Robot Arm 1 togoal: 22.8343371178693
Mobile Platform 2 Pose: [5.37045146185757;4.20858175880155;7.90898921990127]
Robot Arm 2 end: [3.63036883326528;7.66489783148187;1.14982349949502]
Robot Arm 1 togoal: 22.3153163079099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.399853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.16430562262588;-5.87066763804866;-7.12398945388242]
Robot Arm 1 end: [-5.64317939007557;-6.55385541637249;0.345886612831504]
Robot Arm 1 togoal: 22.8061413230944
Mobile Platform 2 Pose: [5.40314382926133;4.1633044499201;7.92798344200599]
Robot Arm 2 end: [3.4847647883034;7.70015497358287;1.00098552778347]
Robot Arm 1 togoal: 22.2576904133388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.787123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.1961960284144;-5.85059748630371;-7.12834915255017]
Robot Arm 1 end: [-5.65637774871473;-6.56007891412322;0.347617146385133]
Robot Arm 1 togoal: 22.8196232478452
Mobile Platform 2 Pose: [5.40484743594401;4.10332558486063;7.9656233219121]
Robot Arm 2 end: [3.35854045178674;7.79293851193477;0.859158346962307]
Robot Arm 1 togoal: 22.2593193576356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.931720e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.21930021519266;-5.83566336195777;-7.13208066981134]
Robot Arm 1 end: [-5.67934784318066;-6.56889083679557;0.332825043816656]
Robot Arm 1 togoal: 22.8425447810791
Mobile Platform 2 Pose: [5.38197188667776;4.0372955688156;8.01587104983281]
Robot Arm 2 end: [3.2469798411274;7.91985102235868;0.727914107699143]
Robot Arm 1 togoal: 22.2987111916212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.029406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.24057963150121;-5.82223919459525;-7.13523128719467]
Robot Arm 1 end: [-5.72321667737682;-6.57936140989732;0.314319710157508]
Robot Arm 1 togoal: 22.8812334625043
Mobile Platform 2 Pose: [5.27847116463589;3.9972871096925;8.06963023606777]
Robot Arm 2 end: [3.19135996370351;8.18448784304375;0.546753623858164]
Robot Arm 1 togoal: 22.4863676881623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.25792988341716;-5.81125037204903;-7.1377412399631]
Robot Arm 1 end: [-5.76946102369834;-6.5931389825201;0.292297224778481]
Robot Arm 1 togoal: 22.9241765998477
Mobile Platform 2 Pose: [5.5306529687062;3.76613056245544;8.10894332830974]
Robot Arm 2 end: [2.99769600715708;8.0141789874777;0.434972977317327]
Robot Arm 1 togoal: 22.2402832891433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.923042e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002054e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.27281546667845;-5.80177582641234;-7.13987742830591]
Robot Arm 1 end: [-5.8199480233169;-6.61043599435835;0.269161606013509]
Robot Arm 1 togoal: 22.972688975124
Mobile Platform 2 Pose: [5.18334806010084;3.75612886576799;8.18207076071725]
Robot Arm 2 end: [3.13927048506455;8.46480975793918;0.194057181720686]
Robot Arm 1 togoal: 22.7013304790613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002490e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.28651162962229;-5.79301008994518;-7.1417801511318]
Robot Arm 1 end: [-5.89132916606615;-6.64210697783473;0.246100631417158]
Robot Arm 1 togoal: 23.0460286324124
Mobile Platform 2 Pose: [5.06486249749554;3.78804410826711;8.20419896464358]
Robot Arm 2 end: [3.24924128706841;8.67494907089618;0.212323208427858]
Robot Arm 1 togoal: 22.9348806935707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000403e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.29810532228515;-5.78558677664456;-7.143236637758]
Robot Arm 1 end: [-5.96416134142092;-6.68386435144611;0.224495619688284]
Robot Arm 1 togoal: 23.127617703528
Mobile Platform 2 Pose: [5.23766498985187;3.68618435043871;8.20360188742564]
Robot Arm 2 end: [3.04230418361642;8.51250950400834;0.229842840775208]
Robot Arm 1 togoal: 22.6821911058829

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000559e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.30773439539718;-5.77941438128787;-7.14432160255882]
Robot Arm 1 end: [-6.07691310082058;-6.71283658766289;0.241408612181646]
Robot Arm 1 togoal: 23.2254902160759
Mobile Platform 2 Pose: [5.54763958595819;3.42818648405276;8.24711532368796]
Robot Arm 2 end: [2.78621167425415;8.2329224532515;0.179135141409698]
Robot Arm 1 togoal: 22.3098021386079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967124e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000781e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.31425522932506;-5.77538506269509;-7.14491688187329]
Robot Arm 1 end: [-6.17053537463746;-6.76796395046927;0.250991375837843]
Robot Arm 1 togoal: 23.329461015036
Mobile Platform 2 Pose: [5.32303184418766;3.52955978686059;8.26690798681053]
Robot Arm 2 end: [2.85543387048413;8.42934209888284;0.200577427600121]
Robot Arm 1 togoal: 22.5088059649051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996093e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.32432427278394;-5.76907950907023;-7.14573363902567]
Robot Arm 1 end: [-6.21995210675734;-6.77744960766151;0.289266464528103]
Robot Arm 1 togoal: 23.3685007902845
Mobile Platform 2 Pose: [5.27933645183743;3.55049498826433;8.27021104616005]
Robot Arm 2 end: [2.88058085246596;8.55520944265592;0.217737465098977]
Robot Arm 1 togoal: 22.6252879272865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980615e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.33896648919439;-5.75971665280096;-7.14688161562157]
Robot Arm 1 end: [-6.25019790203451;-6.77122816005935;0.340001440487326]
Robot Arm 1 togoal: 23.38242467003
Mobile Platform 2 Pose: [5.2386796311869;3.57027912169443;8.272988354268]
Robot Arm 2 end: [2.93207160520453;8.65768221369294;0.215724903219436]
Robot Arm 1 togoal: 22.738762601944

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.3587533840738;-5.7468456524183;-7.14838934268313]
Robot Arm 1 end: [-6.27554470585843;-6.7707287256751;0.4079652659003]
Robot Arm 1 togoal: 23.3963574896156
Mobile Platform 2 Pose: [5.18776740297024;3.59460312470959;8.2766665739887]
Robot Arm 2 end: [2.98725935022384;8.7414193995013;0.226270832671012]
Robot Arm 1 togoal: 22.8382580367836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974674e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.37587315554335;-5.73549238136615;-7.14976920120971]
Robot Arm 1 end: [-6.26219648923357;-6.80327253935508;0.349344428426922]
Robot Arm 1 togoal: 23.4132821164544
Mobile Platform 2 Pose: [5.14387529798874;3.6127793459019;8.28213471729217]
Robot Arm 2 end: [2.9106368773949;8.64422862914878;0.256756904073467]
Robot Arm 1 togoal: 22.713214575694

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995846e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988364e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.42021010764855;-5.70797091867005;-7.15214600626744]
Robot Arm 1 end: [-6.22536267947741;-6.83559444747912;0.314325857669803]
Robot Arm 1 togoal: 23.412725107649
Mobile Platform 2 Pose: [5.12437441068834;3.61059118737693;8.28779793287106]
Robot Arm 2 end: [2.84995762082513;8.5984597260181;0.326661266750678]
Robot Arm 1 togoal: 22.6373708731173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994794e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997905e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.45537439452188;-5.68405334790434;-7.15518845074601]
Robot Arm 1 end: [-6.19004428522174;-6.87851104089527;0.287658289277735]
Robot Arm 1 togoal: 23.4205636287958
Mobile Platform 2 Pose: [5.1043884395771;3.59680814943065;8.2979238901036]
Robot Arm 2 end: [2.77290387327393;8.50202134242546;0.429209450288669]
Robot Arm 1 togoal: 22.5091953147131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002348e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.48179146896418;-5.66531307015726;-7.15844959151886]
Robot Arm 1 end: [-6.16187851534893;-6.93363166390572;0.265417337424504]
Robot Arm 1 togoal: 23.4420708619415
Mobile Platform 2 Pose: [5.02642081871188;3.6333053125279;8.30645686880715]
Robot Arm 2 end: [2.78155026246855;8.36819346734532;0.51169093927004]
Robot Arm 1 togoal: 22.400385334764

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996618e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.50133064580965;-5.6515487230495;-7.16140299733655]
Robot Arm 1 end: [-6.15002830656973;-6.99847776973895;0.252724398106083]
Robot Arm 1 togoal: 23.4814859792491
Mobile Platform 2 Pose: [4.91947423556226;3.66471722120249;8.31934550732312]
Robot Arm 2 end: [2.82834899590139;8.22231393264271;0.573714896099286]
Robot Arm 1 togoal: 22.3050804098192

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998098e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.52128130098791;-5.63874365100235;-7.16380480997522]
Robot Arm 1 end: [-6.16092839135677;-7.07321208560152;0.262092676306934]
Robot Arm 1 togoal: 23.5426214564169
Mobile Platform 2 Pose: [4.88239339463026;3.60824955775934;8.33703095977907]
Robot Arm 2 end: [2.78104098660446;8.18635663154576;0.696365971770192]
Robot Arm 1 togoal: 22.2436192538792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999022e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001601e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.54084903792551;-5.62692317327813;-7.1656248709374]
Robot Arm 1 end: [-6.17338781924061;-7.1414457761339;0.287292640104687]
Robot Arm 1 togoal: 23.599357421285
Mobile Platform 2 Pose: [4.77445511651667;3.60239096834895;8.34446163886882]
Robot Arm 2 end: [2.83873211643581;8.08410533659885;0.88640341918774]
Robot Arm 1 togoal: 22.1871724427242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.55605264961167;-5.61843675807821;-7.16671194793544]
Robot Arm 1 end: [-6.1915995835756;-7.19896328234563;0.32069996356654]
Robot Arm 1 togoal: 23.6519122956217
Mobile Platform 2 Pose: [4.86473140890648;3.49357389880808;8.3429085553622]
Robot Arm 2 end: [2.672519789764;8.20261649833044;0.809813109672562]
Robot Arm 1 togoal: 22.1908492852397

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998539e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.56344614531189;-5.61543528881765;-7.16670090454689]
Robot Arm 1 end: [-6.21982079431232;-7.24019045699746;0.352367393453746]
Robot Arm 1 togoal: 23.6996298655113
Mobile Platform 2 Pose: [5.00033884567291;3.34913850809826;8.34755892541102]
Robot Arm 2 end: [2.51003824479389;8.36900220882497;0.738833580833156]
Robot Arm 1 togoal: 22.238099814087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.361691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.5612918124046;-5.61940998580996;-7.16454658722778]
Robot Arm 1 end: [-6.26283682017234;-7.25624480241764;0.370990985118061]
Robot Arm 1 togoal: 23.7398476586272
Mobile Platform 2 Pose: [5.20728617242543;3.13435703242159;8.36509979762753]
Robot Arm 2 end: [2.31339550278882;8.38458049741444;0.613058476050244]
Robot Arm 1 togoal: 22.145809235269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.527748e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.54702358198417;-5.63229308210594;-7.15756729701829]
Robot Arm 1 end: [-6.31368927790808;-7.23780849799792;0.364372924190777]
Robot Arm 1 togoal: 23.7616557895757
Mobile Platform 2 Pose: [5.3614915194125;2.93526166758644;8.38491867180035]
Robot Arm 2 end: [2.14887896570479;8.40994404944377;0.469672076022409]
Robot Arm 1 togoal: 22.0822686884914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995951e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.570892e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.53664827900735;-5.64592303085166;-7.14083913463923]
Robot Arm 1 end: [-6.33343801325605;-7.19981643684043;0.305669473085178]
Robot Arm 1 togoal: 23.7506191726349
Mobile Platform 2 Pose: [5.51689069253188;2.74109381774328;8.40365038207746]
Robot Arm 2 end: [1.97282559734676;8.44676861793092;0.379461667621712]
Robot Arm 1 togoal: 22.021238271093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996811e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.361847e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-4.52036103789897;-5.66589650989896;-7.10837904223843]
Robot Arm 1 end: [-6.33064461433887;-7.13925806805831;0.22676012052648]
Robot Arm 1 togoal: 23.7090526489136
Mobile Platform 2 Pose: [5.57086463115246;2.58137604558727;8.43103317459121]
Robot Arm 2 end: [1.80303035043141;8.54613009695191;0.383150325044242]
Robot Arm 1 togoal: 22.012841000404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.113998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-4.47440147441349;-5.70740363722567;-7.05453855776022]
Robot Arm 1 end: [-6.37876925537547;-7.11773829057176;0.233492691140629]
Robot Arm 1 togoal: 23.7263297114346
Mobile Platform 2 Pose: [5.45049822074082;2.53958008929095;8.46315682237461]
Robot Arm 2 end: [1.73152766919928;8.44007028224753;0.597659556872137]
Robot Arm 1 togoal: 21.8750426523317

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.962327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-4.39019800643359;-5.77982560552354;-6.99116229232862]
Robot Arm 1 end: [-6.47586785479192;-6.97161755895325;0.268870293891062]
Robot Arm 1 togoal: 23.686659809624
Mobile Platform 2 Pose: [5.59957923920914;2.385102516742;8.47399587754709]
Robot Arm 2 end: [1.6044315166933;8.58640530778016;0.389197563383196]
Robot Arm 1 togoal: 21.9407900916588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.042746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-4.42159706502198;-5.78284203230793;-6.89551672589628]
Robot Arm 1 end: [-6.49161407083696;-6.80692626248261;0.300525035418569]
Robot Arm 1 togoal: 23.5782879826574
Mobile Platform 2 Pose: [5.47135388311458;2.35189130553139;8.50308828346875]
Robot Arm 2 end: [1.54459753489669;8.45737399848819;0.599739988487358]
Robot Arm 1 togoal: 21.7899313255371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-4.56230455164321;-5.70268963629669;-6.83308625774843]
Robot Arm 1 end: [-6.39730325709749;-6.78029372693181;0.261342165183004]
Robot Arm 1 togoal: 23.4954580616796
Mobile Platform 2 Pose: [5.61907957525774;2.19376180958889;8.51326432772227]
Robot Arm 2 end: [1.41023395746522;8.59733624522536;0.387153033656632]
Robot Arm 1 togoal: 21.848111207268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-4.69980484569476;-5.62104117462921;-6.76207638438877]
Robot Arm 1 end: [-6.31861140107725;-6.68608485042659;0.266632895192199]
Robot Arm 1 togoal: 23.3729746187478
Mobile Platform 2 Pose: [5.488728010171;2.16951598786689;8.54075325237519]
Robot Arm 2 end: [1.36115490628454;8.52253229372334;0.597889112338363]
Robot Arm 1 togoal: 21.7488431206705

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.032218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-4.84150195367078;-5.53129372102615;-6.71468354089801]
Robot Arm 1 end: [-6.21443660473231;-6.63261044696322;0.240772581343103]
Robot Arm 1 togoal: 23.2635050440125
Mobile Platform 2 Pose: [5.63317928598414;2.01798672957861;8.55171524757354]
Robot Arm 2 end: [1.24235925682139;8.68751270960493;0.38540419451346]
Robot Arm 1 togoal: 21.8381169235015

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.546353e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-4.97997035590347;-5.44233537790559;-6.67537907827888]
Robot Arm 1 end: [-6.11780628310626;-6.56668908547856;0.233937250489745]
Robot Arm 1 togoal: 23.1494146179948
Mobile Platform 2 Pose: [5.51013151203449;1.99203794684518;8.5813493712387]
Robot Arm 2 end: [1.18352338879467;8.58802572623044;0.596217969015668]
Robot Arm 1 togoal: 21.7126948486721

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.005286e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-5.12017189904288;-5.35778359281461;-6.65116609016499]
Robot Arm 1 end: [-6.03068792274511;-6.54149440845143;0.232047676030948]
Robot Arm 1 togoal: 23.070877543664
Mobile Platform 2 Pose: [5.65009133290721;1.84657187738591;8.59315151749502]
Robot Arm 2 end: [1.07604269828914;8.75281719161093;0.38387358148046]
Robot Arm 1 togoal: 21.8091707996429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.670738e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-5.23396224945349;-5.29261906562665;-6.63537294823264]
Robot Arm 1 end: [-5.96416130438532;-6.52417936172683;0.233108217885127]
Robot Arm 1 togoal: 23.0122057824406
Mobile Platform 2 Pose: [5.52958406152304;1.82076435697492;8.62351065541989]
Robot Arm 2 end: [1.01333028054103;8.66536361780021;0.594854319285626]
Robot Arm 1 togoal: 21.6920950734337

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-5.31331546867606;-5.25006861736036;-6.62673596727951]
Robot Arm 1 end: [-5.91947846074401;-6.51462387302589;0.234729801658911]
Robot Arm 1 togoal: 22.9742701114257
Mobile Platform 2 Pose: [5.66224703257387;1.68235016509536;8.63681399036738]
Robot Arm 2 end: [0.919968652274971;8.83002280964438;0.382673793512408]
Robot Arm 1 togoal: 21.797047069059

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-5.35392683339798;-5.22953779808976;-6.62361674970997]
Robot Arm 1 end: [-5.89324103278485;-6.51251608901114;0.234131104892269]
Robot Arm 1 togoal: 22.9546145615483
Mobile Platform 2 Pose: [5.54730596452552;1.66094345797903;8.66764841489261]
Robot Arm 2 end: [0.855611685600579;8.73613851921512;0.59128471304398]
Robot Arm 1 togoal: 21.6737528405803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-5.34455231035682;-5.23266066168747;-6.62484425098782]
Robot Arm 1 end: [-5.88250870619026;-6.53640978469967;0.223089563592202]
Robot Arm 1 togoal: 22.965006049872
Mobile Platform 2 Pose: [5.67400393507242;1.52736610223559;8.6822046684187]
Robot Arm 2 end: [0.773245065291152;8.9003348396675;0.379297745878652]
Robot Arm 1 togoal: 21.7850625504136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-5.28806447458165;-5.2512590138912;-6.63072472484659]
Robot Arm 1 end: [-5.79900012579399;-6.55614648643;0.114434381354896]
Robot Arm 1 togoal: 22.927992024914
Mobile Platform 2 Pose: [5.56393608099153;1.50967642721622;8.7128999048124]
Robot Arm 2 end: [0.706387045690332;8.80082589662281;0.589248100228301]
Robot Arm 1 togoal: 21.655621431166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-5.2829075814329;-5.24761351366653;-6.63347625439978]
Robot Arm 1 end: [-5.74926353670581;-6.55344794852884;0.0714691882681233]
Robot Arm 1 togoal: 22.8944770743388
Mobile Platform 2 Pose: [5.6850918403576;1.38042969121346;8.72838355599809]
Robot Arm 2 end: [0.629858771773232;8.96532146336827;0.386062812345747]
Robot Arm 1 togoal: 21.7707463844642

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-5.32481392293921;-5.22634471897919;-6.63005860446914]
Robot Arm 1 end: [-5.75154727257129;-6.4836420182643;0.111468248305493]
Robot Arm 1 togoal: 22.8431275316094
Mobile Platform 2 Pose: [5.58137910215108;1.36348865948104;8.758938563746]
Robot Arm 2 end: [0.562315040942281;8.86662467409178;0.595460139339327]
Robot Arm 1 togoal: 21.6418263936026

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.144046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-5.35951281006694;-5.20626763244407;-6.62912166030771]
Robot Arm 1 end: [-5.74192849241383;-6.39642844821318;0.156520948829829]
Robot Arm 1 togoal: 22.7708801234982
Mobile Platform 2 Pose: [5.6979871998511;1.23938289532336;8.77490702495552]
Robot Arm 2 end: [0.49117879338503;9.03137316310451;0.399250516856543]
Robot Arm 1 togoal: 21.7604301164071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.170415e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-5.42503784480629;-5.18515866670783;-6.6174496023524]
Robot Arm 1 end: [-5.7009361509513;-6.26815308378527;0.207466486766512]
Robot Arm 1 togoal: 22.6472420189327
Mobile Platform 2 Pose: [5.60177693780238;1.22203611603026;8.80529609650645]
Robot Arm 2 end: [0.4234207496287;8.93847455676368;0.602187897070449]
Robot Arm 1 togoal: 21.6369529819119

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.082134e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-5.2844511608487;-5.27848004261864;-6.61147971972816]
Robot Arm 1 end: [-5.76063558548672;-6.18863161292154;0.229467278592561]
Robot Arm 1 togoal: 22.6304547971944
Mobile Platform 2 Pose: [5.71053606016024;1.10276517306417;8.8226459079229]
Robot Arm 2 end: [0.355981784271362;9.10498845995542;0.422355098019795]
Robot Arm 1 togoal: 21.7589972974822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.693066e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-5.31913640986577;-5.25735064063918;-6.61084876068675]
Robot Arm 1 end: [-5.65262000121811;-6.13721325231097;0.191725264386073]
Robot Arm 1 togoal: 22.5206824003145
Mobile Platform 2 Pose: [5.62005654974642;1.08752393441897;8.85226191907953]
Robot Arm 2 end: [0.29275627800114;9.01841523297683;0.614895456195596]
Robot Arm 1 togoal: 21.6439824250659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-5.3436162695579;-5.24117042615801;-6.61199123341724]
Robot Arm 1 end: [-5.54983387864513;-6.0903228463913;0.176487243487945]
Robot Arm 1 togoal: 22.4165906753678
Mobile Platform 2 Pose: [5.71853381154;0.973558463698587;8.87163033002918]
Robot Arm 2 end: [0.230577845564154;9.18735525823795;0.452660172187627]
Robot Arm 1 togoal: 21.7706379063102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-5.36283065779757;-5.22684939129857;-6.6153923864449]
Robot Arm 1 end: [-5.41219062935661;-6.06274165661883;0.144693335216982]
Robot Arm 1 togoal: 22.3033968165935
Mobile Platform 2 Pose: [5.63309643443041;0.96223555467463;8.89986458232389]
Robot Arm 2 end: [0.17478397902063;9.1081853880724;0.638546716422851]
Robot Arm 1 togoal: 21.6662802908661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-5.39069750794974;-5.20500075377449;-6.63062657215587]
Robot Arm 1 end: [-5.27589831388632;-6.03389329561419;0.164997656116979]
Robot Arm 1 togoal: 22.1873284977699
Mobile Platform 2 Pose: [5.71950696581078;0.855780276675225;8.92129497407229]
Robot Arm 2 end: [0.122805591323383;9.2789008486999;0.496385610283205]
Robot Arm 1 togoal: 21.7990147855664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-5.46773405304406;-5.1584869460659;-6.64211537088069]
Robot Arm 1 end: [-5.11450779769061;-6.00641761372866;0.214590341469637]
Robot Arm 1 togoal: 22.0535815002333
Mobile Platform 2 Pose: [5.64610749206285;0.842406345660744;8.94924684227709]
Robot Arm 2 end: [0.0733115687321696;9.22122675870369;0.660891004119629]
Robot Arm 1 togoal: 21.7178947311901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-5.54971954295772;-5.1105708229474;-6.64692546008733]
Robot Arm 1 end: [-4.97350277386765;-6.02784699395763;0.211093350775925]
Robot Arm 1 togoal: 21.9730179995926
Mobile Platform 2 Pose: [5.71542895578742;0.748374849728242;8.97299879862863]
Robot Arm 2 end: [0.0402565229375306;9.3911913086986;0.552142506630644]
Robot Arm 1 togoal: 21.8578072152583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999685e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-5.62237050012594;-5.0677803288757;-6.64832290094867]
Robot Arm 1 end: [-4.82206650052972;-6.05952739458233;0.215600209533746]
Robot Arm 1 togoal: 21.8930615514691
Mobile Platform 2 Pose: [5.65709367858154;0.732519146038289;9.00065791865542]
Robot Arm 2 end: [0.00133940542914235;9.36315916354579;0.688990390315183]
Robot Arm 1 togoal: 21.8094237128358

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-5.69241859837192;-5.02618630199265;-6.64837673789869]
Robot Arm 1 end: [-4.78644271097657;-6.02907458896499;0.248489000052809]
Robot Arm 1 togoal: 21.8446653549232
Mobile Platform 2 Pose: [5.7043031348282;0.656652582146902;9.02634426944666]
Robot Arm 2 end: [-0.0152375415517958;9.50547668131432;0.620915710261963]
Robot Arm 1 togoal: 21.931011231714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.748219e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-5.74166408581407;-4.99630582055623;-6.64663090928764]
Robot Arm 1 end: [-4.7774393127361;-5.97586065090459;0.305037119702303]
Robot Arm 1 togoal: 21.7963162462142
Mobile Platform 2 Pose: [5.66653156723797;0.634842177890374;9.0536246741628]
Robot Arm 2 end: [-0.0374507101541131;9.51558170066595;0.718343851340095]
Robot Arm 1 togoal: 21.9261162625979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.769622e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-5.77016982054302;-4.97861634580255;-6.6440163083458]
Robot Arm 1 end: [-4.81681478168431;-5.91752739103683;0.371879343658724]
Robot Arm 1 togoal: 21.77673430067
Mobile Platform 2 Pose: [5.6908615955635;0.580519509376026;9.07915698672413]
Robot Arm 2 end: [-0.0373237280802037;9.62199719681304;0.691902424391492]
Robot Arm 1 togoal: 22.0219230604571

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.802284e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-5.77513321683791;-4.97467046467503;-6.6409650699195]
Robot Arm 1 end: [-4.92132385392181;-5.87107330323433;0.405269716054038]
Robot Arm 1 togoal: 21.8124214019619
Mobile Platform 2 Pose: [5.67792233711388;0.552426611746057;9.10435631592196]
Robot Arm 2 end: [-0.033415772798315;9.67361752972259;0.738436775716483]
Robot Arm 1 togoal: 22.0679956739101

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.855214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-5.79376487083911;-4.96351055176402;-6.63730305239883]
Robot Arm 1 end: [-5.06526494638729;-5.85655249206038;0.418230406199593]
Robot Arm 1 togoal: 21.8999672514669
Mobile Platform 2 Pose: [5.68645793192651;0.518204399328405;9.12658988866354]
Robot Arm 2 end: [-0.0112076265005389;9.75279325360557;0.736899638293612]
Robot Arm 1 togoal: 22.1486710160631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969795e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-5.86062753926403;-4.92592670373345;-6.62510472750696]
Robot Arm 1 end: [-5.18331793650087;-5.85411593470323;0.536552493078339]
Robot Arm 1 togoal: 21.9739680650939
Mobile Platform 2 Pose: [5.68566596776235;0.494566846248695;9.14662749281211]
Robot Arm 2 end: [0.0228284064569422;9.8175003484657;0.751201745948911]
Robot Arm 1 togoal: 22.2212285591652

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-5.7644793732349;-4.97717040048251;-6.59699253006029]
Robot Arm 1 end: [-5.34550768534915;-5.83848366225422;0.560579074794069]
Robot Arm 1 togoal: 22.0740702905961
Mobile Platform 2 Pose: [5.68827795638317;0.47458887288802;9.16346145804388]
Robot Arm 2 end: [0.0720932538230364;9.88154682777714;0.752633777956305]
Robot Arm 1 togoal: 22.300523613359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.831079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-5.68092434106146;-5.02252383933007;-6.58776009220321]
Robot Arm 1 end: [-5.49602628638648;-5.89116379788189;0.501168887308732]
Robot Arm 1 togoal: 22.2192703079535
Mobile Platform 2 Pose: [5.69090662473665;0.459185488392903;9.17742777485888]
Robot Arm 2 end: [0.135921122227421;9.93960295374166;0.747653426821031]
Robot Arm 1 togoal: 22.381303086977

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989474e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-5.61042303290699;-5.0604408976946;-6.57988992123545]
Robot Arm 1 end: [-5.63512282531653;-5.98720973244118;0.442913820076526]
Robot Arm 1 togoal: 22.3877245517077
Mobile Platform 2 Pose: [5.69467975001277;0.446745791518805;9.18892468219771]
Robot Arm 2 end: [0.214217709686127;9.99061776322622;0.732498908578382]
Robot Arm 1 togoal: 22.4627864689013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-5.52939916282156;-5.10446047245996;-6.56999751076909]
Robot Arm 1 end: [-5.74820783305206;-6.01588778844275;0.583855123345913]
Robot Arm 1 togoal: 22.4809464241351
Mobile Platform 2 Pose: [5.70100102553236;0.435074096385898;9.19907584640894]
Robot Arm 2 end: [0.306174776502954;10.0327807915927;0.702825179734566]
Robot Arm 1 togoal: 22.543268503133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-5.40913343099056;-5.17050643554567;-6.55981777183656]
Robot Arm 1 end: [-5.85846354324566;-6.06702771573388;0.581561043150267]
Robot Arm 1 togoal: 22.5947514065603
Mobile Platform 2 Pose: [5.71223979936472;0.420879463684911;9.21013677137852]
Robot Arm 2 end: [0.410155788757762;10.0634537600623;0.649483981791775]
Robot Arm 1 togoal: 22.6201914337248

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-5.21457430208658;-5.27802899144748;-6.54310140812218]
Robot Arm 1 end: [-6.00488032570414;-6.13056843643046;0.69887798313874]
Robot Arm 1 togoal: 22.7382346925412
Mobile Platform 2 Pose: [5.75463664486939;0.384645277913347;9.22978682424577]
Robot Arm 2 end: [0.509650659634133;10.076868723889;0.524679757034581]
Robot Arm 1 togoal: 22.6831670798053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-4.98201693925718;-5.41209011483711;-6.51191944915719]
Robot Arm 1 end: [-6.0368599547502;-6.01260860588934;0.798119637246524]
Robot Arm 1 togoal: 22.6739292490853
Mobile Platform 2 Pose: [5.78235432824779;0.324003391883475;9.30887643525553]
Robot Arm 2 end: [0.595264340197198;10.114645989223;0.44822977131212]
Robot Arm 1 togoal: 22.7598208620801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.867867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-4.81961529789513;-5.50461611174082;-6.4915944452486]
Robot Arm 1 end: [-6.0915424050537;-6.05853314422111;0.959831977814774]
Robot Arm 1 togoal: 22.7404734617996
Mobile Platform 2 Pose: [6.14028391928437;0.0904448816044991;9.36202493433658]
Robot Arm 2 end: [0.483103099982142;10.0398515925982;0.247307607867565]
Robot Arm 1 togoal: 22.6520064181519

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917199e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-4.63326899871264;-5.61415741008587;-6.45964600434291]
Robot Arm 1 end: [-6.13826362206409;-6.09765475450196;1.08417230170444]
Robot Arm 1 togoal: 22.7984398538228
Mobile Platform 2 Pose: [5.86476631619323;0.273489443557744;9.47122916042368]
Robot Arm 2 end: [0.804254447757579;10.0329363144666;0.291053479251544]
Robot Arm 1 togoal: 22.7939294549067

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.936493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-4.45853341112216;-5.71989336574294;-6.42650923580161]
Robot Arm 1 end: [-6.09473196213516;-6.04687391270357;1.18737835962481]
Robot Arm 1 togoal: 22.7300211768953
Mobile Platform 2 Pose: [5.77867410071296;0.33060948999573;9.47766981889649]
Robot Arm 2 end: [0.933739076015346;10.0345501069078;0.372345161890319]
Robot Arm 1 togoal: 22.8527734882595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002250e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.401305e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-4.2773761342389;-5.82950217747069;-6.38109238764284]
Robot Arm 1 end: [-6.03447436102388;-5.98454046181709;1.25580005076357]
Robot Arm 1 togoal: 22.6424431725659
Mobile Platform 2 Pose: [5.74624036113452;0.357914806824571;9.49728762752724]
Robot Arm 2 end: [1.05888491448761;9.99377805949457;0.46253385895673]
Robot Arm 1 togoal: 22.8729195166674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989060e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-4.1438703630264;-5.92547941350356;-6.22456662885678]
Robot Arm 1 end: [-6.06621741172194;-5.96339722507049;1.34363349869126]
Robot Arm 1 togoal: 22.6491688978181
Mobile Platform 2 Pose: [5.94236787380738;0.230507035380661;9.46744499649526]
Robot Arm 2 end: [0.97467420542488;9.82127138073675;0.329295143468469]
Robot Arm 1 togoal: 22.688024011119

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.928131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.339536e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-4.28356458818983;-5.85079344357653;-6.20842840499263]
Robot Arm 1 end: [-6.10892655624404;-5.94342041056175;1.32839460349197]
Robot Arm 1 togoal: 22.6655524093534
Mobile Platform 2 Pose: [5.85504939843554;0.0774986100450921;9.30522928372813]
Robot Arm 2 end: [0.80489577470707;9.55414872215625;0.220487921313597]
Robot Arm 1 togoal: 22.3786022042776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.051839e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-4.4688307629746;-5.75142223325897;-6.19431654679925]
Robot Arm 1 end: [-6.12758009070454;-5.83511386111471;1.257562285104]
Robot Arm 1 togoal: 22.6035189739604
Mobile Platform 2 Pose: [6.00330498904435;-0.102832579955684;9.26403041805619]
Robot Arm 2 end: [0.552714336555635;9.43099597566221;0.166584554072304]
Robot Arm 1 togoal: 22.1530545450354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.014643e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.559329e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-4.82505858864529;-5.5579457245258;-6.15220621180764]
Robot Arm 1 end: [-6.04186474324947;-5.72589615488002;1.16182993639381]
Robot Arm 1 togoal: 22.46718134189
Mobile Platform 2 Pose: [5.96987719810498;-0.0843328715865629;9.24480379433234]
Robot Arm 2 end: [0.534907010678354;9.39545593983888;0.0758843569913278]
Robot Arm 1 togoal: 22.1191395867818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.422905e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-5.38043412462164;-5.21667896596973;-5.68599084346577]
Robot Arm 1 end: [-5.79186960186455;-5.24554401508972;1.07021752607599]
Robot Arm 1 togoal: 21.9547936149974
Mobile Platform 2 Pose: [6.03013377188143;-0.0403041225412013;9.22127724296929]
Robot Arm 2 end: [0.592477509930325;9.37643233849821;0.0688208756702527]
Robot Arm 1 togoal: 22.1304210121705

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-5.12380394420394;-5.35856164379938;-5.69592933831938]
Robot Arm 1 end: [-5.81216329805549;-5.26286820859046;1.18314350416039]
Robot Arm 1 togoal: 21.9793949218116
Mobile Platform 2 Pose: [6.07947213248705;0.111818287561151;9.22883164055518]
Robot Arm 2 end: [0.785563402875664;9.36337550252552;0.139374983006328]
Robot Arm 1 togoal: 22.2076017718447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.822696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-5.62985327415847;-5.14564214655105;-5.82694571029006]
Robot Arm 1 end: [-5.55133480833091;-5.20093909952131;1.03326560985847]
Robot Arm 1 togoal: 21.7520217143944
Mobile Platform 2 Pose: [6.29174538718585;0.178902677455433;9.21417149823129]
Robot Arm 2 end: [0.859333772912269;9.4551207528421;0.230509832252003]
Robot Arm 1 togoal: 22.3179797108124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.016903e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983502e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-5.76698409069212;-5.07000630940537;-5.82670479283111]
Robot Arm 1 end: [-5.3463095166835;-5.06057011322736;0.860894629660417]
Robot Arm 1 togoal: 21.5119903081337
Mobile Platform 2 Pose: [6.4539786271515;0.151958440354718;9.22840154168102]
Robot Arm 2 end: [0.895215850656136;9.46459230507514;0.314776494609192]
Robot Arm 1 togoal: 22.3390030480354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.996137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.424861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-5.95532023648477;-4.96240944803472;-5.82230181976482]
Robot Arm 1 end: [-5.19568732981534;-4.99885457188361;0.729880563997479]
Robot Arm 1 togoal: 21.365870491418
Mobile Platform 2 Pose: [6.60248464930499;0.115334708171017;9.24062737700842]
Robot Arm 2 end: [0.907890861768697;9.4340642801023;0.384979835954933]
Robot Arm 1 togoal: 22.3149160721819

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.061604e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-6.05714683682403;-4.90208479541366;-5.81807037862049]
Robot Arm 1 end: [-5.04479101762309;-4.96305108966101;0.605568999535612]
Robot Arm 1 togoal: 21.2385651255533
Mobile Platform 2 Pose: [6.76898259499192;0.0776685823567958;9.2684146150433]
Robot Arm 2 end: [0.8816233736117;9.45007850811155;0.396741536492284]
Robot Arm 1 togoal: 22.3154484227766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.201034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-6.20328017292092;-4.81322169153366;-5.80720722613292]
Robot Arm 1 end: [-4.85333119904714;-4.94676392399809;0.519311196243588]
Robot Arm 1 togoal: 21.0957479120293
Mobile Platform 2 Pose: [6.94201686454227;0.0470065266886711;9.30962489662924]
Robot Arm 2 end: [0.866881260411437;9.51152228892844;0.465919503643637]
Robot Arm 1 togoal: 22.3584839649969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.044549e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-6.30190987875208;-4.75313553226692;-5.79915453772746]
Robot Arm 1 end: [-4.67862765784694;-4.96865364090693;0.443704810275866]
Robot Arm 1 togoal: 20.9924573755658
Mobile Platform 2 Pose: [7.20490173605149;-0.0276636131376438;9.39316152780297]
Robot Arm 2 end: [0.809630238046467;9.60468827812498;0.599653400522281]
Robot Arm 1 togoal: 22.4062667109474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.432031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-6.4015909487111;-4.69293672911862;-5.79179704219262]
Robot Arm 1 end: [-4.48592483610464;-5.02692515167179;0.357786976057138]
Robot Arm 1 togoal: 20.9046301667906
Mobile Platform 2 Pose: [7.3887744420011;-0.238855485496294;9.52694490886971]
Robot Arm 2 end: [0.58398809559051;9.7593307290462;0.503193858004037]
Robot Arm 1 togoal: 22.4385356966562

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.994942e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-6.49760536126802;-4.63476747557936;-5.78557636037181]
Robot Arm 1 end: [-4.26700488651035;-5.12491011312773;0.305884705543192]
Robot Arm 1 togoal: 20.827550012921
Mobile Platform 2 Pose: [7.68112392721789;-0.423495081727318;9.63976293398021]
Robot Arm 2 end: [0.418248693697749;9.87503871431812;0.608376204149677]
Robot Arm 1 togoal: 22.4586275611875

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.117095e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-6.58654522354804;-4.57959137473252;-5.78054740705459]
Robot Arm 1 end: [-4.11366031550635;-5.22499541334019;0.280464802744203]
Robot Arm 1 togoal: 20.7974714908654
Mobile Platform 2 Pose: [7.41449000370166;-0.371178600244652;9.62853035378284]
Robot Arm 2 end: [0.461340779574492;9.88700073284041;0.527826301827788]
Robot Arm 1 togoal: 22.4926397527975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.989913e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-6.71470447516671;-4.49763974158867;-5.7713020826794]
Robot Arm 1 end: [-3.96641195893034;-5.35255795889411;0.234395048613048]
Robot Arm 1 togoal: 20.7946399629493
Mobile Platform 2 Pose: [7.35347159621107;-0.441141390944839;9.60363955243883]
Robot Arm 2 end: [0.382541948066021;9.85788275582923;0.460909949756714]
Robot Arm 1 togoal: 22.4333786078298

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.503157e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-6.91591713229182;-4.35663533128623;-5.74738969478474]
Robot Arm 1 end: [-3.84603892802776;-5.51055934749627;0.122346798735502]
Robot Arm 1 togoal: 20.8385814307517
Mobile Platform 2 Pose: [7.1985051993243;-0.452411817294083;9.56015008497]
Robot Arm 2 end: [0.345243580623912;9.82314745961829;0.379936615327694]
Robot Arm 1 togoal: 22.3893560781915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.166267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-7.11413266819793;-4.12645619064333;-5.64982846156371]
Robot Arm 1 end: [-3.50728585462851;-5.49327134288612;0.222031241027612]
Robot Arm 1 togoal: 20.5955225206774
Mobile Platform 2 Pose: [7.21084873465901;-0.580703553993149;9.50083627252492]
Robot Arm 2 end: [0.169134921480649;9.74865169327022;0.262343335235967]
Robot Arm 1 togoal: 22.2487025543769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.108891e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-7.38727916569505;-3.89872532071577;-5.61104173096937]
Robot Arm 1 end: [-3.31276752099942;-5.3934684476028;0.167591222348895]
Robot Arm 1 togoal: 20.3965772921572
Mobile Platform 2 Pose: [6.92448975127225;-0.537151768970815;9.4059222713169]
Robot Arm 2 end: [0.211729534538144;9.65472806061638;0.26104746378633]
Robot Arm 1 togoal: 22.185022777019

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.594411e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-7.51275257040472;-3.69338313993717;-5.53583061836233]
Robot Arm 1 end: [-3.10123045627468;-5.23921927850709;0.196503264721194]
Robot Arm 1 togoal: 20.1402665148355
Mobile Platform 2 Pose: [6.99518696217857;-0.695541119430646;9.32352218276412]
Robot Arm 2 end: [0.0428616532700056;9.57048425960894;0.255091101635055]
Robot Arm 1 togoal: 22.0332805020333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.765457e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-7.42144612603148;-3.6139787245483;-5.45920046928088]
Robot Arm 1 end: [-3.03797298728155;-5.18293364088289;0.16101650954629]
Robot Arm 1 togoal: 20.0589074641378
Mobile Platform 2 Pose: [6.75504297178512;-0.642095041581412;9.25211634355462]
Robot Arm 2 end: [0.10551338618262;9.503596087081;0.281015092518111]
Robot Arm 1 togoal: 22.0011186919461

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.025376e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-7.34599035626916;-3.51715410661233;-5.37589075139979]
Robot Arm 1 end: [-2.9773754539893;-5.20463934125556;0.0842994922718155]
Robot Arm 1 togoal: 20.0413931531809
Mobile Platform 2 Pose: [6.81071527679018;-0.742013137435133;9.2001806045026]
Robot Arm 2 end: [-0.0329341322418262;9.44539181490766;0.227223367065307]
Robot Arm 1 togoal: 21.8892558764211

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.598230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-7.5656596142974;-3.27152927629718;-5.30864557955476]
Robot Arm 1 end: [-2.75498319845421;-4.90673072047324;0.156590987294779]
Robot Arm 1 togoal: 19.6662611881237
Mobile Platform 2 Pose: [6.64975173355312;-0.728540361076971;9.10918674478757]
Robot Arm 2 end: [0.00664678809913977;9.37384438852385;0.294602670949124]
Robot Arm 1 togoal: 21.8399376542429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.835513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-7.40647505687758;-3.29104466013006;-5.26149838749969]
Robot Arm 1 end: [-2.6873224662043;-5.01569490582015;0.177922968813097]
Robot Arm 1 togoal: 19.7038854233836
Mobile Platform 2 Pose: [6.50622507917434;-0.697664112552052;9.06630354391083]
Robot Arm 2 end: [0.0209704541196296;9.30362488188493;0.254728853347133]
Robot Arm 1 togoal: 21.7865377019781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-7.33129271454504;-3.19817065816999;-5.19310548948503]
Robot Arm 1 end: [-2.55629060830013;-5.02956488088608;0.202696125749219]
Robot Arm 1 togoal: 19.6287080434173
Mobile Platform 2 Pose: [6.49249913897377;-0.752763491528866;9.01396069035155]
Robot Arm 2 end: [-0.0385199150646266;9.26900806976834;0.295561226726181]
Robot Arm 1 togoal: 21.7261928938874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009747e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-7.41158293566818;-3.01846988564138;-5.13474462903498]
Robot Arm 1 end: [-2.41021411515624;-4.89492252242186;0.156844145699959]
Robot Arm 1 togoal: 19.4353557345317
Mobile Platform 2 Pose: [6.40569539872107;-0.721900272012689;8.99638675553942]
Robot Arm 2 end: [-0.0202647125970812;9.25058984247075;0.295110927703844]
Robot Arm 1 togoal: 21.7182671893328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.862282e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-7.27302636620869;-2.97329585775877;-5.07253654786292]
Robot Arm 1 end: [-2.36690602466323;-4.97544926569794;0.134075674705195]
Robot Arm 1 togoal: 19.4712096289994
Mobile Platform 2 Pose: [6.37254008993948;-0.706526121725401;8.9928832621045]
Robot Arm 2 end: [-0.018949314139;9.23787735521684;0.305841389393348]
Robot Arm 1 togoal: 21.7070013676929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010696e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-7.24405909838105;-2.90900565418434;-5.05184769673324]
Robot Arm 1 end: [-2.33846921366573;-5.04407575761208;0.11378735956429]
Robot Arm 1 togoal: 19.507493056586
Mobile Platform 2 Pose: [6.44220105997364;-0.742449972309242;8.99654090479476]
Robot Arm 2 end: [-0.0768221022749396;9.2363592591221;0.31405000278842]
Robot Arm 1 togoal: 21.6786463007191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024333e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-7.40466337945733;-2.75860118856057;-5.03947645821957]
Robot Arm 1 end: [-2.19868598884549;-4.93394507883121;0.106248097205597]
Robot Arm 1 togoal: 19.3347401952667
Mobile Platform 2 Pose: [6.63609092995123;-0.86152342411194;8.98696627341226]
Robot Arm 2 end: [-0.214214950260224;9.21978255854824;0.310289423523019]
Robot Arm 1 togoal: 21.6015613032387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.018260e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-7.54360176504399;-2.62217372679149;-5.02234916543447]
Robot Arm 1 end: [-2.0722844723019;-4.82074280233422;0.11501559229482]
Robot Arm 1 togoal: 19.1669314640564
Mobile Platform 2 Pose: [6.95376242962346;-1.08413319461608;8.93221029876197]
Robot Arm 2 end: [-0.448748891253121;9.15902960089889;0.279064652995461]
Robot Arm 1 togoal: 21.4438153414125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.023527e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-7.59468957268662;-2.54319641259816;-5.0028158030989]
Robot Arm 1 end: [-1.95552367006647;-4.93138479699347;0.124548772622981]
Robot Arm 1 togoal: 19.1789174410823
Mobile Platform 2 Pose: [7.17810972365468;-1.22563617897778;8.91472427768648]
Robot Arm 2 end: [-0.614221303679225;9.11964701891;0.227926642833388]
Robot Arm 1 togoal: 21.3383641025041

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003889e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-7.61248243133145;-2.53997408488611;-5.00517338134957]
Robot Arm 1 end: [-1.9962571084088;-5.09734567147158;0.100643653793067]
Robot Arm 1 togoal: 19.3353935332374
Mobile Platform 2 Pose: [7.26863714149134;-1.27838027557566;8.9138262322079]
Robot Arm 2 end: [-0.696078996392373;9.09017307684264;0.184067700914678]
Robot Arm 1 togoal: 21.2787462534219

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995708e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-7.63958312775273;-2.52870794466934;-5.00650103784528]
Robot Arm 1 end: [-2.02150617993828;-5.23335072850536;0.105026292935898]
Robot Arm 1 togoal: 19.4570441432157
Mobile Platform 2 Pose: [7.26742371227285;-1.27921480092546;8.91242031005845]
Robot Arm 2 end: [-0.799537155608064;9.08191054592604;0.0914233364098648]
Robot Arm 1 togoal: 21.2323224370496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989271e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984750e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-7.68716392824138;-2.50225526501025;-5.00884981337713]
Robot Arm 1 end: [-2.02829139297079;-5.34853718621871;0.119795010437159]
Robot Arm 1 togoal: 19.5504670791705
Mobile Platform 2 Pose: [7.21941388836227;-1.25560559816555;8.9062824123033]
Robot Arm 2 end: [-0.788616420204474;9.15311313986174;0.0901685183557647]
Robot Arm 1 togoal: 21.3011410288589
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 87)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',87,0)">第 87 行</a>)
    real_time_2D_plot(human_poses{k}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000996e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.35244564369129;9.995589454957;9.81851213643726]
Robot Arm 2 end: [9.77354328089878;9.83010419669133;1.48915679394973]
Robot Arm 1 togoal: 28.0040543550927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994556e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.15525396590624;9.86183206708906;9.81488129082644]
Robot Arm 2 end: [9.54885243433075;9.88320375905613;1.46026994816734]
Robot Arm 1 togoal: 27.8837427789488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883228e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-2.00072694105967;9.74634955903048;9.8279949507694]
Robot Arm 2 end: [9.25003359813043;9.99270689298932;1.34409911099651]
Robot Arm 1 togoal: 27.7543404959056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.89602312325615;9.6539899698337;9.85216938794193]
Robot Arm 2 end: [9.00461970920958;9.9916523211819;1.20808614456591]
Robot Arm 1 togoal: 27.5851345456863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.926096e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.75725177568135;9.52821108407023;9.87441331374949]
Robot Arm 2 end: [8.81503207650961;10.0357366102956;1.12917588010485]
Robot Arm 1 togoal: 27.4879919182569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821037e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.59121191036488;9.44623701446103;9.86658333093286]
Robot Arm 2 end: [8.67983410734389;10.0009302332738;1.04316093146792]
Robot Arm 1 togoal: 27.3715498766436

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.50565997404019;9.3983580241991;9.86191343164896]
Robot Arm 2 end: [8.63495993641619;9.83621068031326;1.06408877808811]
Robot Arm 1 togoal: 27.2203187683451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919726e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.44165527512258;9.36027638838783;9.85462996777543]
Robot Arm 2 end: [8.65245552763546;9.70032394457919;1.11249413865439]
Robot Arm 1 togoal: 27.1327060532217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954466e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.37912248909797;9.3242609715937;9.8441762858014]
Robot Arm 2 end: [8.66807756929182;9.60349748139573;1.12150589364735]
Robot Arm 1 togoal: 27.0731200488085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980737e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.31737162964614;9.28899932202491;9.82958632386522]
Robot Arm 2 end: [8.63949439653706;9.51481592048351;1.0598583422459]
Robot Arm 1 togoal: 26.9902101399268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.26339632472718;9.25926434008467;9.81567029990104]
Robot Arm 2 end: [8.629927790643;9.37871214538798;0.985467834757416]
Robot Arm 1 togoal: 26.8867158388374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.18836138214678;9.22184321821229;9.80285845477093]
Robot Arm 2 end: [8.67083529142629;9.22646346216227;0.909268895068989]
Robot Arm 1 togoal: 26.8072975640248

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.06064823658747;9.15857218882472;9.78214899531732]
Robot Arm 2 end: [8.73625363237017;9.08336508694529;0.827011168607438]
Robot Arm 1 togoal: 26.7526380702211

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.846968101636209;9.04649060877302;9.73255731664238]
Robot Arm 2 end: [8.7999909713281;8.95290897410512;0.740281864367063]
Robot Arm 1 togoal: 26.7069642585981

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993778e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.657014527228857;8.90101740409313;9.57722764541427]
Robot Arm 2 end: [8.75025039117633;8.7713559191365;0.666280834955474]
Robot Arm 1 togoal: 26.545550241534

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.891055850496795;8.95637819545918;9.43610164996899]
Robot Arm 2 end: [8.69073817517897;8.64030514914831;0.734643566789756]
Robot Arm 1 togoal: 26.4087679763087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-0.913315504817845;8.92932858535374;9.2656125512257]
Robot Arm 2 end: [8.76970324473665;8.43887221959059;0.806831183022575]
Robot Arm 1 togoal: 26.3211664962822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.08447840777179;9.00529049783333;9.22318715380929]
Robot Arm 2 end: [8.761615421549;8.44533599269838;0.926945496605184]
Robot Arm 1 togoal: 26.316943854913

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999598e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.28519732295049;9.09259698950951;9.14743111955246]
Robot Arm 2 end: [8.82686921673466;8.46115202628649;1.07941510481214]
Robot Arm 1 togoal: 26.3715781829861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.42810932018891;9.1377370353389;8.96270801531774]
Robot Arm 2 end: [8.90220760809152;8.32389244802432;1.13222082865194]
Robot Arm 1 togoal: 26.328874570521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.338808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.36535720103045;9.10787480264859;8.80884396097298]
Robot Arm 2 end: [8.76807080458935;8.17022390434687;1.04124045397932]
Robot Arm 1 togoal: 26.1271446522994

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.167520e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.40802513087632;9.09620654018298;8.60600785831056]
Robot Arm 2 end: [8.780396252064;7.94061868137728;0.995756413114366]
Robot Arm 1 togoal: 25.9777782514342
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.49890893318775;9.11497758444294;8.44098341904581]
Robot Arm 2 end: [8.84791737868862;7.73267745834894;0.906788286961322]
Robot Arm 1 togoal: 25.8856928704669
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.48643524464244;9.0730384129534;8.24663293298274]
Robot Arm 2 end: [8.79716114500048;7.54125540636499;0.904754687098864]
Robot Arm 1 togoal: 25.717866681125
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.50138444148934;9.04705905167707;8.07203051332853]
Robot Arm 2 end: [8.73757095675442;7.37101140114545;0.81807418866066]
Robot Arm 1 togoal: 25.5605693824371
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.47226684627367;8.9767708810964;7.88222037789367]
Robot Arm 2 end: [8.63723674043553;7.18614724189935;0.789162661944472]
Robot Arm 1 togoal: 25.3622916202116
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-1.50388406324365;8.93483677787251;7.70998336831025]
Robot Arm 2 end: [8.61255862742211;7.01019129535227;0.706750825228652]
Robot Arm 1 togoal: 25.2277020016156

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.394948e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-1.39236465980603;8.81249017452524;7.53215657802795]
Robot Arm 2 end: [8.43178275230957;6.85195096002564;0.637719269290693]
Robot Arm 1 togoal: 24.9899790203331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.777151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-1.46880727074267;8.77572712785924;7.3720877357301]
Robot Arm 2 end: [8.53315332166921;6.64928630947709;0.559694011964446]
Robot Arm 1 togoal: 24.9319196184745

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-1.27883526763113;8.63048968902729;7.21132758466506]
Robot Arm 2 end: [8.24296597753109;6.55430361434952;0.48219310871543]
Robot Arm 1 togoal: 24.6562344158795

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-1.36080042659486;8.58669703556153;7.04836420632908]
Robot Arm 2 end: [8.35738815380083;6.34898197845272;0.404621545671054]
Robot Arm 1 togoal: 24.6075031731049

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998825e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-1.1890536989189;8.44803082418285;6.88090975579043]
Robot Arm 2 end: [8.07070407716769;6.23715603491071;0.382796762570107]
Robot Arm 1 togoal: 24.3205898069167

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.862564e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-1.28744192034246;8.40958160769373;6.71423010115833]
Robot Arm 2 end: [8.12271666734864;6.06776874172378;0.308955601768321]
Robot Arm 1 togoal: 24.25025986182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000968e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-1.13269075208239;8.2557190970743;6.53682258631884]
Robot Arm 2 end: [7.86256193779978;6.01783831379957;0.179282910731301]
Robot Arm 1 togoal: 24.030032625371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.828753e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-1.30242612841893;8.22908059417997;6.37586321935736]
Robot Arm 2 end: [7.76407797068179;5.92076132208378;0.202685667069102]
Robot Arm 1 togoal: 23.8908153880028

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008063e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-1.11005037144822;7.98575115836089;6.22483937633158]
Robot Arm 2 end: [7.42550343784432;5.88633666667079;0.178910630973347]
Robot Arm 1 togoal: 23.6183205716153

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.898515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-1.27188597336082;7.89328719566178;6.1054970363756]
Robot Arm 2 end: [7.45370993209858;5.71183754241575;0.123021465258057]
Robot Arm 1 togoal: 23.5254792371314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-1.04327457777775;7.62643598446737;5.96712117840861]
Robot Arm 2 end: [7.06985228942888;5.66528515590899;0.148528247080917]
Robot Arm 1 togoal: 23.2091511027746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-1.20221577351598;7.51879826925966;5.85403145441386]
Robot Arm 2 end: [7.09746875754258;5.47773506909305;0.0952801549492212]
Robot Arm 1 togoal: 23.1066310365823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.118235e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-0.97996785841242;7.24800780536354;5.71873245848098]
Robot Arm 2 end: [6.69924355188372;5.4229142315091;0.138863144892177]
Robot Arm 1 togoal: 22.7736980063376

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.426898e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-1.06205813795464;7.08789738092356;5.60515346606894]
Robot Arm 2 end: [6.64460533482863;5.22348313494693;0.117797271018039]
Robot Arm 1 togoal: 22.6001394555618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.989409e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [-0.92491030772636;6.91098479643479;5.52119628545847]
Robot Arm 2 end: [6.42716317250378;5.0726791026853;0.170461777330936]
Robot Arm 1 togoal: 22.335203126891
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-0.778473254053385;6.69488425980322;5.39549601468198]
Robot Arm 2 end: [6.07221434058534;5.0224700167377;0.238636818300859]
Robot Arm 1 togoal: 22.0371308146791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.426306e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-1.00880690639857;6.63235805169501;5.28782152049128]
Robot Arm 2 end: [6.07456328432615;4.80044357939123;0.278423442705986]
Robot Arm 1 togoal: 21.8858107327393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.734336e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-0.801610881878874;6.36183440789504;5.15468536246456]
Robot Arm 2 end: [5.72757676186939;4.85451778915707;0.201959050326057]
Robot Arm 1 togoal: 21.6737915136183

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.171682e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-0.967461414625847;6.25208965508339;5.05890765689848]
Robot Arm 2 end: [5.62927455694386;4.61971438617531;0.27885354753877]
Robot Arm 1 togoal: 21.4371919737412

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.473973e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.738106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307232452701;-6.24962959492604;-7.10439671995214]
Robot Arm 1 end: [-6.13860510704698;-6.93244391788516;1.24046676848776]
Robot Arm 1 togoal: 23.3931812870228
Mobile Platform 2 Pose: [-0.767500575923866;5.97245657636037;4.94338409166937]
Robot Arm 2 end: [5.30609685292731;4.66403234012162;0.212897137020795]
Robot Arm 1 togoal: 21.2372686201276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.668052e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.139269e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407590539037;-6.21793451945078;-7.10337765534701]
Robot Arm 1 end: [-6.10318028168213;-6.88095764811569;1.17439360336278]
Robot Arm 1 togoal: 23.33236555294
Mobile Platform 2 Pose: [-0.791707847861579;5.77878424063305;4.84891474214762]
Robot Arm 2 end: [5.13799641847459;4.41887546656209;0.284070633147451]
Robot Arm 1 togoal: 20.9426096641407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.953648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.641517e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859253518329;-6.18978076208593;-7.10219156926314]
Robot Arm 1 end: [-6.07851988729173;-6.82867678863295;1.10278390529305]
Robot Arm 1 togoal: 23.2787041934078
Mobile Platform 2 Pose: [-0.52040513008681;5.54021322907858;4.77418942868816]
Robot Arm 2 end: [4.8764873527915;4.470814916498;0.227321656572774]
Robot Arm 1 togoal: 20.7939405587676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000058e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.767492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70845333614463;-6.16355517958648;-7.1010800588953]
Robot Arm 1 end: [-6.0585955679198;-6.77642347375278;1.02303773723625]
Robot Arm 1 togoal: 23.2287503452044
Mobile Platform 2 Pose: [-0.207048772879039;5.2886259967365;4.70303215632311]
Robot Arm 2 end: [4.6334658813429;4.48386418048598;0.214604660566064]
Robot Arm 1 togoal: 20.6307371192133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000259e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.717281e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74535993669749;-6.13815955978458;-7.10000571195893]
Robot Arm 1 end: [-6.04159065483294;-6.72130381293;0.930971654433173]
Robot Arm 1 togoal: 23.1793607631898
Mobile Platform 2 Pose: [-0.121535648577742;5.05768233739495;4.59971145350018]
Robot Arm 2 end: [4.46591061563469;4.31601714307629;0.185737169423261]
Robot Arm 1 togoal: 20.3959271328152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.747667e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78013115264505;-6.1134873398428;-7.09883182831411]
Robot Arm 1 end: [-6.0289698796036;-6.6592529312289;0.819170857911064]
Robot Arm 1 togoal: 23.1290109553475
Mobile Platform 2 Pose: [-0.202183530925203;4.96726336627086;4.52025593528782]
Robot Arm 2 end: [4.45159866254688;4.07434996056914;0.196341030006874]
Robot Arm 1 togoal: 20.2161013081821

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977954e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.277569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80724295199038;-6.09291218019221;-7.09790615250934]
Robot Arm 1 end: [-5.99722757036452;-6.60138684949264;0.729717343745151]
Robot Arm 1 togoal: 23.0682349456266
Mobile Platform 2 Pose: [0.0728658153457393;4.73499364537297;4.41711992627202]
Robot Arm 2 end: [4.21583490961731;3.99917385562785;0.196171522937837]
Robot Arm 1 togoal: 19.9955495376152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82883561937563;-6.0752938634081;-7.09842981814034]
Robot Arm 1 end: [-5.97781419485215;-6.56354744587666;0.646591203868351]
Robot Arm 1 togoal: 23.030550171018
Mobile Platform 2 Pose: [-0.0279914671991571;4.66047023839602;4.30341410068129]
Robot Arm 2 end: [4.18017749505182;3.75409178452021;0.263036380234627]
Robot Arm 1 togoal: 19.7948212186126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.758828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85468187276528;-6.05590488426347;-7.10073559026391]
Robot Arm 1 end: [-5.94854374757573;-6.53420529050897;0.572545101234337]
Robot Arm 1 togoal: 22.9920760035284
Mobile Platform 2 Pose: [0.0170438530150433;4.48536654598098;4.12498007388809]
Robot Arm 2 end: [4.13056462565004;3.5340493239953;0.182109126468913]
Robot Arm 1 togoal: 19.6121384992875

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.663134e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.8948872940016;-6.02935147045575;-7.10425144458682]
Robot Arm 1 end: [-5.91354564205304;-6.51037690061649;0.51150184241101]
Robot Arm 1 togoal: 22.953269197051
Mobile Platform 2 Pose: [0.286184993290804;4.27268898048343;3.98219532542612]
Robot Arm 2 end: [3.92228423700731;3.47375653547512;0.118136810332303]
Robot Arm 1 togoal: 19.4252448808813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984286e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003702e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94495864876022;-5.99853713187141;-7.10838744153295]
Robot Arm 1 end: [-5.88428226655825;-6.49016346351707;0.462516593796477]
Robot Arm 1 togoal: 22.9206522878041
Mobile Platform 2 Pose: [0.631765442376962;4.03496858983042;3.84647920864837]
Robot Arm 2 end: [3.57027210006067;3.36372996569643;0.202208008696773]
Robot Arm 1 togoal: 19.0913796196625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982175e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99692299982983;-5.96812234916634;-7.11341319378366]
Robot Arm 1 end: [-5.85823512792285;-6.47355461444532;0.424596207481033]
Robot Arm 1 togoal: 22.8924381463366
Mobile Platform 2 Pose: [0.599659913527637;3.9345881848456;3.69182720187705]
Robot Arm 2 end: [3.56355231827105;3.15605289643676;0.144851496477603]
Robot Arm 1 togoal: 18.9458561170527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007084e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.046718624883;-5.94083921712702;-7.11957544648321]
Robot Arm 1 end: [-5.80769100660457;-6.4698963934272;0.388646896151747]
Robot Arm 1 togoal: 22.8565702965757
Mobile Platform 2 Pose: [0.768912814638318;3.76114391200914;3.51266834078972]
Robot Arm 2 end: [3.29372011586552;3.01824380815803;0.197509065471937]
Robot Arm 1 togoal: 18.6533990098692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.746825e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943710e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.07836188212081;-5.92508716394414;-7.12403342375029]
Robot Arm 1 end: [-5.7592243268006;-6.47668967673635;0.373742010130789]
Robot Arm 1 togoal: 22.8287243397701
Mobile Platform 2 Pose: [0.773899633624189;3.64015785675542;3.35191826398709]
Robot Arm 2 end: [3.26581940045331;2.80048592688381;0.146679658451472]
Robot Arm 1 togoal: 18.4857556169876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.827077e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005867e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.11922807989448;-5.89817244261755;-7.12343597670438]
Robot Arm 1 end: [-5.75807122833621;-6.48442440608137;0.379613038167962]
Robot Arm 1 togoal: 22.8332174919298
Mobile Platform 2 Pose: [0.958025769105356;3.46469132067489;3.18005953999227]
Robot Arm 2 end: [2.99532310997503;2.67494020604635;0.194948204780451]
Robot Arm 1 togoal: 18.2013995429776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.413658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.934499e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.1653795178671;-5.87005553974399;-7.12400787436447]
Robot Arm 1 end: [-5.64036097589073;-6.55535209125668;0.345709986580981]
Robot Arm 1 togoal: 22.8053037896294
Mobile Platform 2 Pose: [0.981759697685072;3.33882736303762;3.02056942394863]
Robot Arm 2 end: [2.95319302244666;2.44146878865065;0.173582313936574]
Robot Arm 1 togoal: 18.0108547691787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.071698e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004460e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.19701112462115;-5.85015346571657;-7.12839623386011]
Robot Arm 1 end: [-5.65292286951051;-6.5618850000019;0.347230558874292]
Robot Arm 1 togoal: 22.8185837381081
Mobile Platform 2 Pose: [1.11134001312828;3.1914843423522;2.85322622924276]
Robot Arm 2 end: [2.71931072165267;2.33708673922719;0.193703298161869]
Robot Arm 1 togoal: 17.7692512779766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.497947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.922932e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.21979521134788;-5.83542673148882;-7.13215469034429]
Robot Arm 1 end: [-5.67549298363658;-6.57074918543521;0.332091517395167]
Robot Arm 1 togoal: 22.8412852577663
Mobile Platform 2 Pose: [1.1989758394587;3.03512679471147;2.69490876874722]
Robot Arm 2 end: [2.57816059230976;2.11374220246978;0.2290519920701]
Robot Arm 1 togoal: 17.510625451109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.033684e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001888e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.23896330965919;-5.82310799373526;-7.13532256097728]
Robot Arm 1 end: [-5.71966994287293;-6.58110787457549;0.313223409508468]
Robot Arm 1 togoal: 22.8801198806674
Mobile Platform 2 Pose: [1.16932461449118;2.95395033836981;2.54664937623893]
Robot Arm 2 end: [2.63968473887557;1.90435575386101;0.19347558402595]
Robot Arm 1 togoal: 17.4137198004233

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.907500e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.25586965494468;-5.81239468665507;-7.13783826934593]
Robot Arm 1 end: [-5.76545207579138;-6.59455430619548;0.290818381962512]
Robot Arm 1 togoal: 22.9225229542628
Mobile Platform 2 Pose: [1.40353108318956;2.77489790816012;2.39750521308339]
Robot Arm 2 end: [2.30532708251039;1.7924248380197;0.275492498734812]
Robot Arm 1 togoal: 17.0889919861768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917187e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.27018728760046;-5.80327330457435;-7.13996095511143]
Robot Arm 1 end: [-5.8148895037187;-6.61106492147144;0.267190008934971]
Robot Arm 1 togoal: 22.969768116676
Mobile Platform 2 Pose: [1.34196354783347;2.71432367663866;2.25981811139194]
Robot Arm 2 end: [2.39958531953187;1.61168767533161;0.194125872309054]
Robot Arm 1 togoal: 17.0394131858035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.888596e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.28310541264335;-5.79499233075413;-7.14183406070799]
Robot Arm 1 end: [-5.88675597939156;-6.64167030719381;0.243350989711989]
Robot Arm 1 togoal: 23.0427123654217
Mobile Platform 2 Pose: [1.56997517621143;2.53914503527623;2.11419975891257]
Robot Arm 2 end: [2.06975850082676;1.50371543822571;0.276787717696394]
Robot Arm 1 togoal: 16.7201256979542

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.29381998586829;-5.78812102694873;-7.14323412068022]
Robot Arm 1 end: [-5.95502627255797;-6.68024084450018;0.219275573060657]
Robot Arm 1 togoal: 23.1189918567449
Mobile Platform 2 Pose: [1.52391474356809;2.47239514374542;1.9794824361863]
Robot Arm 2 end: [2.15551273713925;1.32205143633244;0.204036097923936]
Robot Arm 1 togoal: 16.6637250502126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964758e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.30512881138261;-5.78086277966527;-7.14451076949377]
Robot Arm 1 end: [-6.01996513836282;-6.72612359536065;0.202921606382213]
Robot Arm 1 togoal: 23.1978409386167
Mobile Platform 2 Pose: [1.73757093317733;2.30369941445008;1.83497049746429]
Robot Arm 2 end: [1.83170287679967;1.21679479735152;0.287795428248418]
Robot Arm 1 togoal: 16.350121467639

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974835e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.31781870347293;-5.77270425458125;-7.14573577451034]
Robot Arm 1 end: [-6.11298925147821;-6.78009309416121;0.213952646785056]
Robot Arm 1 togoal: 23.3003931222311
Mobile Platform 2 Pose: [1.68187780262256;2.24424393063153;1.70627923710691]
Robot Arm 2 end: [1.91120651845955;1.01444672781946;0.268846882142489]
Robot Arm 1 togoal: 16.2715323456621

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.939159e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.32960077462401;-5.76518388264173;-7.14664666221321]
Robot Arm 1 end: [-6.17998241311509;-6.79282364841972;0.248444305483028]
Robot Arm 1 togoal: 23.35401796204
Mobile Platform 2 Pose: [1.8010655297706;2.11356628492335;1.55337459448599]
Robot Arm 2 end: [1.7102968339416;0.850929567335043;0.357577173917253]
Robot Arm 1 togoal: 16.0071008332721

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.026351e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.34474871553962;-5.75524687334322;-7.14797360791377]
Robot Arm 1 end: [-6.22658184058113;-6.79040333649453;0.309359526756119]
Robot Arm 1 togoal: 23.3813531487732
Mobile Platform 2 Pose: [1.81232799323609;2.03042393395637;1.41144546777228]
Robot Arm 2 end: [1.71034924585616;0.676968030108589;0.346940627112604]
Robot Arm 1 togoal: 15.8905110423469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978442e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.751915e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.35405991902498;-5.74894799548394;-7.14896137484165]
Robot Arm 1 end: [-6.27450871433783;-6.80054351957665;0.360769151606779]
Robot Arm 1 togoal: 23.4193089579477
Mobile Platform 2 Pose: [2.39699713892195;1.79719957434383;1.31962258849161]
Robot Arm 2 end: [1.32419041629902;0.636760841250879;0.437858411105639]
Robot Arm 1 togoal: 15.5740542403113

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.35826733255705;-5.74572226502816;-7.14978979122921]
Robot Arm 1 end: [-6.27018964433514;-6.84876521339347;0.288891455551284]
Robot Arm 1 togoal: 23.4545955357152
Mobile Platform 2 Pose: [2.31431610992688;1.75116102779945;1.1925850840127]
Robot Arm 2 end: [1.41943028460033;0.422092170795837;0.490316025321502]
Robot Arm 1 togoal: 15.494699180593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.36247391471803;-5.74175501604431;-7.15134517976634]
Robot Arm 1 end: [-6.34427506912388;-6.89107831078287;0.342844719518381]
Robot Arm 1 togoal: 23.5336334615187
Mobile Platform 2 Pose: [2.30946590222843;1.67595304891565;1.03633726045188]
Robot Arm 2 end: [1.45981135191948;0.210630561553613;0.586088204397546]
Robot Arm 1 togoal: 15.3772010701451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.35488911249302;-5.74294835323083;-7.15339247431138]
Robot Arm 1 end: [-6.39986972206576;-6.92298461270188;0.405998917720709]
Robot Arm 1 togoal: 23.5920826235404
Mobile Platform 2 Pose: [2.49905711337831;1.53519764249558;0.894534142158202]
Robot Arm 2 end: [1.20974849801722;0.116442711738774;0.660460082710354]
Robot Arm 1 togoal: 15.1241992673312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.35115381444742;-5.74246682006953;-7.15529878580212]
Robot Arm 1 end: [-6.40450300496938;-6.95163614632589;0.401296136078388]
Robot Arm 1 togoal: 23.6160840026528
Mobile Platform 2 Pose: [2.48320124139917;1.47407851245265;0.740156082401147]
Robot Arm 2 end: [1.26806875186162;-0.0743517881166882;0.754936607480976]
Robot Arm 1 togoal: 15.0357812881174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.30256375347956;-5.76588648747505;-7.15658593414638]
Robot Arm 1 end: [-6.44966912807012;-6.94749932877982;0.437824141133093]
Robot Arm 1 togoal: 23.6428132391898
Mobile Platform 2 Pose: [2.64104027914566;1.34686549970428;0.582746996965098]
Robot Arm 2 end: [1.03807354775684;-0.165782064377013;0.835801434951165]
Robot Arm 1 togoal: 14.7993380637298

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.27884303939318;-5.7780850379536;-7.15724576225554]
Robot Arm 1 end: [-6.46138124584534;-6.95432569291443;0.419530466812466]
Robot Arm 1 togoal: 23.6567002634566
Mobile Platform 2 Pose: [2.59541098871274;1.2954745871691;0.420166152343811]
Robot Arm 2 end: [1.0980320613735;-0.342986554956634;0.927130180080935]
Robot Arm 1 togoal: 14.723345600645

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.24877883716408;-5.79423316996119;-7.15752310382163]
Robot Arm 1 end: [-6.47670067470197;-6.95836530163799;0.400446796867116]
Robot Arm 1 togoal: 23.6711524499458
Mobile Platform 2 Pose: [2.76422468273606;1.15205070923556;0.243050286868726]
Robot Arm 2 end: [0.871776193727785;-0.447422150458128;0.987034177489777]
Robot Arm 1 togoal: 14.4821400586265

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.2154701410507;-5.81258267698829;-7.15749099392941]
Robot Arm 1 end: [-6.49446391487418;-6.96183487431166;0.382010312066931]
Robot Arm 1 togoal: 23.6868841271223
Mobile Platform 2 Pose: [2.6930880358433;1.10671907041996;0.0696561774484225]
Robot Arm 2 end: [0.921636932018081;-0.613574347759394;1.07284874655276]
Robot Arm 1 togoal: 14.4079137749304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.13032847921828;-5.85726397792149;-7.15722148463668]
Robot Arm 1 end: [-6.53907880254172;-6.96592482760217;0.365121280597906]
Robot Arm 1 togoal: 23.7217155472891
Mobile Platform 2 Pose: [2.92061305393718;0.934437008356891;-0.11150367210798]
Robot Arm 2 end: [0.697125186674943;-0.731236143235276;1.10905071869387]
Robot Arm 1 togoal: 14.1600908507761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.04672387864582;-5.90143487018474;-7.15679082418836]
Robot Arm 1 end: [-6.58271185576288;-6.96725494025088;0.350264544040162]
Robot Arm 1 togoal: 23.7538353578231
Mobile Platform 2 Pose: [2.72879249547229;0.978761184559717;-0.22681558546393]
Robot Arm 2 end: [0.808933480972423;-0.819157519930732;1.18759660589883]
Robot Arm 1 togoal: 14.1856289766485

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-3.95588145263088;-5.94943468010802;-7.15621400808965]
Robot Arm 1 end: [-6.62916685270345;-6.96353526042701;0.337731864061447]
Robot Arm 1 togoal: 23.784256184046
Mobile Platform 2 Pose: [3.0248576464283;0.787360934475955;-0.387644514042944]
Robot Arm 2 end: [0.588577255407213;-0.932541652134409;1.21654537896553]
Robot Arm 1 togoal: 13.9437965346699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.588582e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-3.87406432917209;-5.99312681605524;-7.15546914536052]
Robot Arm 1 end: [-6.6703438795776;-6.95456547199084;0.327840228503739]
Robot Arm 1 togoal: 23.8071687868951
Mobile Platform 2 Pose: [2.81804356221088;0.839662896235989;-0.507337149932167]
Robot Arm 2 end: [0.855889297629391;-1.02621877880239;1.28359749428124]
Robot Arm 1 togoal: 14.0867115324346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.624175e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-3.76023414585987;-6.05304122337996;-7.15452165083536]
Robot Arm 1 end: [-6.72688823480169;-6.94066435955316;0.320950344346201]
Robot Arm 1 togoal: 23.837261129136
Mobile Platform 2 Pose: [3.12590352234573;0.65422526220539;-0.744837097229923]
Robot Arm 2 end: [0.661040129658195;-1.07573685643944;1.37790166862659]
Robot Arm 1 togoal: 13.9039823947723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.956389e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-3.59259529716514;-6.140068471681;-7.15333778479294]
Robot Arm 1 end: [-6.80973171677672;-6.92219040163442;0.317437357345573]
Robot Arm 1 togoal: 23.8825495166234
Mobile Platform 2 Pose: [2.90164556812634;0.740250764288597;-0.827209133905916]
Robot Arm 2 end: [1.00703199923095;-1.1636371014252;1.34110288933117]
Robot Arm 1 togoal: 14.1162468693995

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.360877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-3.42926624435922;-6.22513593689309;-7.15188143058234]
Robot Arm 1 end: [-6.88947912490843;-6.89872832900213;0.317680294552827]
Robot Arm 1 togoal: 23.9221550568015
Mobile Platform 2 Pose: [2.3379208228425;0.995291915747581;-0.999407257718711]
Robot Arm 2 end: [1.50656742249493;-1.26866391722849;1.24578523884312]
Robot Arm 1 togoal: 14.4469050942923

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.675057e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-3.39149988308819;-6.24764999462455;-7.15008497502479]
Robot Arm 1 end: [-6.90458560635974;-6.86868253325846;0.32220323549642]
Robot Arm 1 togoal: 23.9113901438872
Mobile Platform 2 Pose: [2.85136495828502;0.819158833435164;-1.17781073371141]
Robot Arm 2 end: [1.38647375288919;-1.503677944604;1.1378610339205]
Robot Arm 1 togoal: 14.2121802852895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.432281e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-3.21858045229919;-6.33810044696402;-7.14779950130422]
Robot Arm 1 end: [-6.98585158733447;-6.83374416946246;0.331988671783418]
Robot Arm 1 togoal: 23.9438477452663
Mobile Platform 2 Pose: [3.26290835853042;0.711344455565904;-1.34668346036345]
Robot Arm 2 end: [1.15169272936388;-1.59222395698891;1.2627120832839]
Robot Arm 1 togoal: 13.9684523612138

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.895265e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-3.04601295024904;-6.42873867854351;-7.14491287634843]
Robot Arm 1 end: [-7.0643037018047;-6.79294236653467;0.347739869835431]
Robot Arm 1 togoal: 23.9701880152454
Mobile Platform 2 Pose: [3.61310693571161;0.593869999977677;-1.38538244105178]
Robot Arm 2 end: [0.929414755763192;-1.66573776431055;1.3109203629135]
Robot Arm 1 togoal: 13.7461328151103

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.270810e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-2.87666478545264;-6.5182664668817;-7.1411787349882]
Robot Arm 1 end: [-7.13730201405491;-6.74573381621521;0.370722012376442]
Robot Arm 1 togoal: 23.9881191595605
Mobile Platform 2 Pose: [3.92575326060757;0.525490245437942;-1.48456468422213]
Robot Arm 2 end: [0.835290567354623;-1.73800896138186;1.31519411734803]
Robot Arm 1 togoal: 13.6274125813576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.036658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-2.76450150211377;-6.57993748693201;-7.13615816230457]
Robot Arm 1 end: [-7.17564687729243;-6.69087327028982;0.402740616279982]
Robot Arm 1 togoal: 23.9757919624691
Mobile Platform 2 Pose: [3.83366184528169;0.598922719152543;-1.52970994454542]
Robot Arm 2 end: [1.04335760733357;-1.70614957219026;1.26334623145985]
Robot Arm 1 togoal: 13.8134024637949

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.126001e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-2.6502705556681;-6.64386462798433;-7.12893557739179]
Robot Arm 1 end: [-7.20642328593324;-6.62782060887112;0.446643648028096]
Robot Arm 1 togoal: 23.9520283497354
Mobile Platform 2 Pose: [4.14174588178166;0.496401554656573;-1.55096586567682]
Robot Arm 2 end: [0.810268967405373;-1.7941572680397;1.34232174383642]
Robot Arm 1 togoal: 13.5731240327545

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.837623e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-2.53440479628878;-6.71075814503129;-7.11750333987543]
Robot Arm 1 end: [-7.22606251195632;-6.55643327830116;0.505701457171341]
Robot Arm 1 togoal: 23.9140867209211
Mobile Platform 2 Pose: [4.45588288272175;0.391530388416881;-1.58481809157878]
Robot Arm 2 end: [0.645005540005046;-1.92627057015411;1.34280393636635]
Robot Arm 1 togoal: 13.3616242908635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.976276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-2.41530632420676;-6.78334485441175;-7.09690645106203]
Robot Arm 1 end: [-7.19146123533226;-6.45262365757475;0.561619719585888]
Robot Arm 1 togoal: 23.8150283465329
Mobile Platform 2 Pose: [4.72892260005784;0.319959556106028;-1.6523577605598]
Robot Arm 2 end: [0.507150715594919;-2.12977625403759;1.2897136763439]
Robot Arm 1 togoal: 13.1298945667986

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.506455e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-2.39386019613563;-6.81453665784909;-7.05044858642349]
Robot Arm 1 end: [-7.05615707511242;-6.38714442460618;0.375659723863045]
Robot Arm 1 togoal: 23.680430758581
Mobile Platform 2 Pose: [4.8989539577955;0.306108565226214;-1.76685402773829]
Robot Arm 2 end: [0.44680169415514;-2.26157061408526;1.28102849449197]
Robot Arm 1 togoal: 13.0029435637185

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.902132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.326216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-2.35978388140335;-6.84811881000331;-6.98587202604043]
Robot Arm 1 end: [-6.984560556577;-6.32039800763982;0.29654221573659]
Robot Arm 1 togoal: 23.5866484540142
Mobile Platform 2 Pose: [5.12895489560558;0.245366891748046;-1.85303445800551]
Robot Arm 2 end: [0.360857032724534;-2.37362821283921;1.2845808723642]
Robot Arm 1 togoal: 12.8671985720463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.854046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.979678e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-2.30788866558741;-6.88987775409325;-6.87874294750514]
Robot Arm 1 end: [-6.89870186057331;-6.26176934677013;0.19353866267013]
Robot Arm 1 togoal: 23.4898578212428
Mobile Platform 2 Pose: [5.34023617041157;0.190044535768974;-1.92290156692556]
Robot Arm 2 end: [0.33355355189593;-2.51742525869448;1.21690293149282]
Robot Arm 1 togoal: 12.7618074267009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007790e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.959546e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-2.18415130215352;-6.95950813948229;-6.75274098934788]
Robot Arm 1 end: [-6.8567498978299;-6.0875684291516;0.277786611526952]
Robot Arm 1 togoal: 23.3346406573982
Mobile Platform 2 Pose: [5.48641516734962;0.165061372030647;-2.0074905421391]
Robot Arm 2 end: [0.363864335698545;-2.62617186587338;1.17511886422071]
Robot Arm 1 togoal: 12.7240975743072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-2.21030052849072;-6.94131744131658;-6.61373952695114]
Robot Arm 1 end: [-6.83899937629282;-5.97135202250887;0.205536201046484]
Robot Arm 1 togoal: 23.2457830676457
Mobile Platform 2 Pose: [5.58159251828007;0.163853742593615;-2.09986598731084]
Robot Arm 2 end: [0.455787078483024;-2.68608641811487;1.14819450425831]
Robot Arm 1 togoal: 12.765414182529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-2.03641928623804;-7.03286881393927;-6.50098845815787]
Robot Arm 1 end: [-6.81056744130238;-5.81635020997251;0.38273637966262]
Robot Arm 1 togoal: 23.1094732898103
Mobile Platform 2 Pose: [5.65007719245357;0.179456563075529;-2.20656363064674]
Robot Arm 2 end: [0.483133996104423;-2.7312024796668;1.19126869622115]
Robot Arm 1 togoal: 12.7608870579037

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-2.1926873901918;-6.94394649851417;-6.43007961644032]
Robot Arm 1 end: [-6.84402214398791;-5.79459424838854;0.19490193087017]
Robot Arm 1 togoal: 23.1289607537246
Mobile Platform 2 Pose: [5.79804852626889;0.149600504598786;-2.32080203881046]
Robot Arm 2 end: [0.622979606330963;-2.78451823981384;1.09163553804847]
Robot Arm 1 togoal: 12.848938733969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-2.01536434057557;-7.03060628273323;-6.29705105747025]
Robot Arm 1 end: [-6.81083429531529;-5.62662609529926;0.345708593829073]
Robot Arm 1 togoal: 22.9820996150538
Mobile Platform 2 Pose: [5.64062161206564;0.249449094765059;-2.40707853211391]
Robot Arm 2 end: [0.632014996634858;-2.8449032641129;1.1897817404417]
Robot Arm 1 togoal: 12.8197058360961

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-2.18917773797004;-6.92873090694987;-6.21670273552566]
Robot Arm 1 end: [-6.83508030258583;-5.60351450981129;0.163290638332459]
Robot Arm 1 togoal: 22.9941942360058
Mobile Platform 2 Pose: [5.80885038711778;0.215493485961037;-2.52935242566589]
Robot Arm 2 end: [0.745155887380322;-2.85245129394926;1.11664205067148]
Robot Arm 1 togoal: 12.9115968377524

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-1.97695727414556;-7.02608351449493;-6.07870448223086]
Robot Arm 1 end: [-6.80559844031107;-5.4181350840771;0.324583790342743]
Robot Arm 1 togoal: 22.8380918676357
Mobile Platform 2 Pose: [5.61395597023347;0.329235033646229;-2.62103271174324]
Robot Arm 2 end: [0.724849935729503;-2.98096580474849;1.22718343580613]
Robot Arm 1 togoal: 12.8209038429988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-2.16026292704955;-6.91638806722542;-5.99202217324426]
Robot Arm 1 end: [-6.83159020785176;-5.38162854486128;0.157991310931467]
Robot Arm 1 togoal: 22.8419508427721
Mobile Platform 2 Pose: [5.76829423624773;0.279137086739007;-2.7730803464508]
Robot Arm 2 end: [0.663721718940691;-2.95076978787059;1.29052877739444]
Robot Arm 1 togoal: 12.7851368575802

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-1.92535052730089;-7.01981004925907;-5.84884645939973]
Robot Arm 1 end: [-6.80251851981146;-5.18305884785361;0.333391762805397]
Robot Arm 1 togoal: 22.677308436211
Mobile Platform 2 Pose: [6.06514041577253;0.144808562963497;-2.98392590059788]
Robot Arm 2 end: [0.445992254998457;-3.07595411952918;1.33502252499451]
Robot Arm 1 togoal: 12.533784471083

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980706e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-2.09799516212766;-6.91379444939542;-5.75302619532259]
Robot Arm 1 end: [-6.82112208965615;-5.08167948788716;0.250561242730466]
Robot Arm 1 togoal: 22.6278858748768
Mobile Platform 2 Pose: [6.57649942148202;-0.0658448400206696;-3.11855555487654]
Robot Arm 2 end: [0.124124248007369;-3.08742044677996;1.37901627606488]
Robot Arm 1 togoal: 12.2597637516613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-1.92652101766659;-6.97592961889012;-5.58037214152229]
Robot Arm 1 end: [-6.76698158381867;-4.90691443311292;0.338035234364522]
Robot Arm 1 togoal: 22.4665790672501
Mobile Platform 2 Pose: [6.34611835581443;-0.179921978238116;-2.99426983521109]
Robot Arm 2 end: [0.124654877140788;-2.98819649414966;1.36425945398089]
Robot Arm 1 togoal: 12.3165982472691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-2.1081107116122;-6.85981980782317;-5.46483020057608]
Robot Arm 1 end: [-6.77139949602996;-4.8027091203357;0.225925697802995]
Robot Arm 1 togoal: 22.4070804837425
Mobile Platform 2 Pose: [6.5436263038009;-0.322038883455994;-2.95288872213139]
Robot Arm 2 end: [-0.128182414412506;-2.90583160368471;1.41657776515048]
Robot Arm 1 togoal: 12.1569285463691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-1.87548126729579;-6.95349964618152;-5.31272235759786]
Robot Arm 1 end: [-6.73680159353265;-4.62533332131961;0.377313345300434]
Robot Arm 1 togoal: 22.2559861948447
Mobile Platform 2 Pose: [6.68025302549617;-0.437695060608487;-2.89792801895641]
Robot Arm 2 end: [-0.386400665490392;-2.85928371882744;1.45287718208097]
Robot Arm 1 togoal: 11.9756320038238

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-2.04513964535325;-6.84579926526812;-5.20433711398384]
Robot Arm 1 end: [-6.74641257681445;-4.49964221812585;0.308100939212324]
Robot Arm 1 togoal: 22.184523496758
Mobile Platform 2 Pose: [6.76749289162616;-0.557942286837796;-2.79941067288777]
Robot Arm 2 end: [-0.636822069052795;-2.81059411261025;1.46023748240252]
Robot Arm 1 togoal: 11.805098477512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-1.86838613676255;-6.90681669559345;-5.03289618782846]
Robot Arm 1 end: [-6.69159888675917;-4.33863949003583;0.381826663409347]
Robot Arm 1 togoal: 22.0341501245788
Mobile Platform 2 Pose: [6.99235128665905;-0.752090653811183;-2.7448364366043]
Robot Arm 2 end: [-0.941618623861521;-2.7117813141968;1.45066301111893]
Robot Arm 1 togoal: 11.6265795511693

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.218628e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-2.04012580625961;-6.7941051361074;-4.91230035979032]
Robot Arm 1 end: [-6.69524485023289;-4.20468075005448;0.301927865402739]
Robot Arm 1 togoal: 21.9542773210495
Mobile Platform 2 Pose: [7.14442316910914;-1.08581846744288;-2.61854330408394]
Robot Arm 2 end: [-1.37084655461694;-2.4864042493295;1.4320059122531]
Robot Arm 1 togoal: 11.442217015776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.522469e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.177374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-1.82395139734015;-6.87656266788388;-4.75644344238883]
Robot Arm 1 end: [-6.63074940333923;-4.06213278309932;0.409183225297008]
Robot Arm 1 togoal: 21.8073735389362
Mobile Platform 2 Pose: [7.3221081501796;-1.28607500883778;-2.57104938949989]
Robot Arm 2 end: [-1.6778213961491;-2.33947282676089;1.37562192849089]
Robot Arm 1 togoal: 11.3120938853072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.817662e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-2.00384320700361;-6.75952340840572;-4.64862212834584]
Robot Arm 1 end: [-6.6313195970328;-3.93932425151287;0.314273310105242]
Robot Arm 1 togoal: 21.7338846373944
Mobile Platform 2 Pose: [7.50703286578622;-1.43602829167728;-2.54900400947904]
Robot Arm 2 end: [-1.8725857874709;-2.17478486838189;1.29160185990553]
Robot Arm 1 togoal: 11.2845680973976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.453608e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-1.81222907676621;-6.82239090084838;-4.47913544989533]
Robot Arm 1 end: [-6.55257263626864;-3.79730538285603;0.391622884834645]
Robot Arm 1 togoal: 21.5784334319013
Mobile Platform 2 Pose: [7.62412674201557;-1.51281472637954;-2.54185048411201]
Robot Arm 2 end: [-1.98211327345077;-2.09464604319515;1.22354599559767]
Robot Arm 1 togoal: 11.2636409578288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.734611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-2.00386128811915;-6.69682160985295;-4.37474710706893]
Robot Arm 1 end: [-6.54681327614871;-3.68913389706169;0.269809754626259]
Robot Arm 1 togoal: 21.5117385043035
Mobile Platform 2 Pose: [7.69509932974169;-1.55089696214147;-2.54322487577138]
Robot Arm 2 end: [-2.07368613550851;-2.13229526115509;1.21032064154957]
Robot Arm 1 togoal: 11.1724550122093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.593515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-1.79276414996342;-6.76614504763699;-4.20845907746132]
Robot Arm 1 end: [-6.49482180484724;-3.52591307666476;0.395222896075437]
Robot Arm 1 togoal: 21.3611058940982
Mobile Platform 2 Pose: [7.76396454115171;-1.58544323033638;-2.54655375284955]
Robot Arm 2 end: [-2.14699619120979;-2.17886689339629;1.20676519590809]
Robot Arm 1 togoal: 11.08776512216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.003450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-1.982825782537;-6.64236325479268;-4.10622882622885]
Robot Arm 1 end: [-6.48850469260863;-3.41109847163588;0.289673442895823]
Robot Arm 1 togoal: 21.2895400209662
Mobile Platform 2 Pose: [7.8331896467897;-1.61481213726578;-2.55378562208461]
Robot Arm 2 end: [-2.19971182198086;-2.23319353398073;1.21312689661784]
Robot Arm 1 togoal: 11.0119282684065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.259527e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-1.79131094746905;-6.70018909327296;-3.93428763475673]
Robot Arm 1 end: [-6.36559522749988;-3.28851053907059;0.369246787715529]
Robot Arm 1 togoal: 21.1126444331203
Mobile Platform 2 Pose: [7.90675191484498;-1.6373403564545;-2.56696042621372]
Robot Arm 2 end: [-2.22366721318473;-2.29848974738712;1.23070321352462]
Robot Arm 1 togoal: 10.9484649155321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.369928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-2.0022338792563;-6.56248574415961;-3.84640599883103]
Robot Arm 1 end: [-6.35599799867534;-3.21576840176168;0.20131882279923]
Robot Arm 1 togoal: 21.0693322958069
Mobile Platform 2 Pose: [7.98587190954548;-1.66399315519512;-2.57843975334841]
Robot Arm 2 end: [-2.20269551949964;-2.36441658146981;1.27953823078974]
Robot Arm 1 togoal: 10.9159697273744

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.079873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-1.78841822412928;-6.61948776642661;-3.66872185996362]
Robot Arm 1 end: [-6.29579281932603;-3.04889433393151;0.303263455597249]
Robot Arm 1 togoal: 20.9119436982812
Mobile Platform 2 Pose: [8.07024337596276;-1.69890235933761;-2.58715387483729]
Robot Arm 2 end: [-2.16211187175007;-2.34524043812998;1.30504922690663]
Robot Arm 1 togoal: 10.9578496520714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.860834e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-2.01111592366403;-6.47515963338167;-3.57623216146219]
Robot Arm 1 end: [-6.27798211655723;-2.97310076282551;0.160230348763357]
Robot Arm 1 togoal: 20.8596677642073
Mobile Platform 2 Pose: [8.14433011742731;-1.7287530954246;-2.59901539296544]
Robot Arm 2 end: [-2.07824210523849;-2.29586838348874;1.31966050666197]
Robot Arm 1 togoal: 11.0520780715358

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.756764e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-1.77474678054355;-6.5465688223277;-3.40481976792202]
Robot Arm 1 end: [-6.21924527191444;-2.78572380977561;0.305320672214784]
Robot Arm 1 togoal: 20.6885602872309
Mobile Platform 2 Pose: [8.22830682736466;-1.76278754186124;-2.6311077682266]
Robot Arm 2 end: [-1.93911882914314;-2.28120880599057;1.33168194253251]
Robot Arm 1 togoal: 11.1621406582374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.025842e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-1.9817266905623;-6.41325829346103;-3.31966492794576]
Robot Arm 1 end: [-6.19828142399534;-2.67709474527985;0.238833222395976]
Robot Arm 1 togoal: 20.609148549063
Mobile Platform 2 Pose: [8.37232417320246;-1.84279260125424;-2.63864454143018]
Robot Arm 2 end: [-1.86803845840017;-2.28245208040609;1.35122670036096]
Robot Arm 1 togoal: 11.2124113097298

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.241414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-1.8830716090298;-6.40229491804285;-3.11799477539802]
Robot Arm 1 end: [-6.10674230397367;-2.53678371060116;0.20926108514575]
Robot Arm 1 togoal: 20.4528421602904
Mobile Platform 2 Pose: [8.51879968212315;-1.92596119398841;-2.64446364876884]
Robot Arm 2 end: [-1.80268739247389;-2.29160348155951;1.34668253345489]
Robot Arm 1 togoal: 11.2536970569744

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.471792e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-2.08154713356605;-6.24267925481173;-2.95685530124264]
Robot Arm 1 end: [-6.0489016659631;-2.34655187625188;0.15834184065161]
Robot Arm 1 togoal: 20.2943692973192
Mobile Platform 2 Pose: [8.65552541411914;-2.00834636584173;-2.65350084008941]
Robot Arm 2 end: [-1.78440985873532;-2.30138110231764;1.31835589425375]
Robot Arm 1 togoal: 11.260804542065
Step 133:
Mobile Platform 1 Pose: [-1.80735351994296;-6.3422067580735;-2.82899827356924]
Robot Arm 1 end: [-6.00859988939372;-2.19139539971033;0.335013724161547]
Robot Arm 1 togoal: 20.1571812428537
Mobile Platform 2 Pose: [8.78219360583497;-2.08530686399326;-2.66133116163788]
Robot Arm 2 end: [-1.82485098708961;-2.29886025509729;1.28305253888541]
Robot Arm 1 togoal: 11.2337542143337
Step 134:
Mobile Platform 1 Pose: [-2.03812752816552;-6.18814856396749;-2.72527395439983]
Robot Arm 1 end: [-5.96976389007373;-2.07148304245372;0.244066579859115]
Robot Arm 1 togoal: 20.0595269263366
Mobile Platform 2 Pose: [8.89856939835819;-2.15362001155415;-2.66634904670437]
Robot Arm 2 end: [-1.87351403079448;-2.29617743811926;1.22528973419565]
Robot Arm 1 togoal: 11.2016115284337
Step 135:
Mobile Platform 1 Pose: [-1.85135715770729;-6.22331918896423;-2.54289936846186]
Robot Arm 1 end: [-5.90422917669058;-1.92884954078253;0.279085900974764]
Robot Arm 1 togoal: 19.9194454666961
Mobile Platform 2 Pose: [9.00769625943155;-2.21601997175664;-2.66902124696597]
Robot Arm 2 end: [-1.89245364779088;-2.30298326975359;1.15152239863464]
Robot Arm 1 togoal: 11.1853872573471
Step 136:
Mobile Platform 1 Pose: [-2.08119279131108;-6.06641083714752;-2.42974779187237]
Robot Arm 1 end: [-5.86849886164994;-1.82212483692035;0.158385875515528]
Robot Arm 1 togoal: 19.835034888655
Mobile Platform 2 Pose: [9.11080947122196;-2.27486177549652;-2.67068639788669]
Robot Arm 2 end: [-1.91305249124741;-2.3229904083775;1.07264216565707]
Robot Arm 1 togoal: 11.1595797871963
Step 137:
Mobile Platform 1 Pose: [-1.81922383942262;-6.15348219308769;-2.28247446649571]
Robot Arm 1 end: [-5.82276678374373;-1.65618013218637;0.311947718901604]
Robot Arm 1 togoal: 19.6898017101531
Mobile Platform 2 Pose: [9.20922178416978;-2.33098970235622;-2.67154966683896]
Robot Arm 2 end: [-1.94427430754796;-2.35298468033418;0.991673844796269]
Robot Arm 1 togoal: 11.1198806607723
Step 138:
Mobile Platform 1 Pose: [-2.04826854923276;-6.00292337401658;-2.18401992421812]
Robot Arm 1 end: [-5.78308734106768;-1.53343062541746;0.253441076554663]
Robot Arm 1 togoal: 19.5890932314769
Mobile Platform 2 Pose: [9.30820738669928;-2.38723636010785;-2.67163906170833]
Robot Arm 2 end: [-1.99567470392236;-2.38299699093586;0.909078586254595]
Robot Arm 1 togoal: 11.0662733610787
Step 139:
Mobile Platform 1 Pose: [-1.92395252371595;-6.00156690097375;-1.98899842625823]
Robot Arm 1 end: [-5.704413667533;-1.40923228879495;0.207052294604015]
Robot Arm 1 togoal: 19.4557356230289
Mobile Platform 2 Pose: [9.41672320458162;-2.44831223635649;-2.67074204388991]
Robot Arm 2 end: [-2.07198433639478;-2.40674221278033;0.824337558557557]
Robot Arm 1 togoal: 10.999833396141
Step 140:
Mobile Platform 1 Pose: [-2.13987401807161;-5.83615055158701;-1.83851581503747]
Robot Arm 1 end: [-5.64070966358373;-1.22746122200791;0.158417262272317]
Robot Arm 1 togoal: 19.3014070986901
Mobile Platform 2 Pose: [9.5364238411536;-2.51536618817562;-2.66903639645964]
Robot Arm 2 end: [-2.15316595474367;-2.43550485314516;0.744153335295486]
Robot Arm 1 togoal: 10.9269337922881
Step 141:
Mobile Platform 1 Pose: [-1.86726987213258;-5.93309327615181;-1.71601006560272]
Robot Arm 1 end: [-5.59683137240916;-1.08163969159338;0.329839292190198]
Robot Arm 1 togoal: 19.1698396171041
Mobile Platform 2 Pose: [9.67128290085798;-2.59116376654453;-2.66659957773848]
Robot Arm 2 end: [-2.20694774758257;-2.45347764831036;0.653785047844344]
Robot Arm 1 togoal: 10.8827259814213
Step 142:
Mobile Platform 1 Pose: [-2.10148934184242;-5.76901456106634;-1.59665924353912]
Robot Arm 1 end: [-5.54298085164376;-0.937043740150241;0.263508808593399]
Robot Arm 1 togoal: 19.0469016804568
Mobile Platform 2 Pose: [9.82085226909023;-2.67508485431153;-2.66253125590132]
Robot Arm 2 end: [-2.30673521462169;-2.43962645400596;0.568603578446266]
Robot Arm 1 togoal: 10.8283253625248
Step 143:
Mobile Platform 1 Pose: [-1.91591599431478;-5.80055393023734;-1.42166858328909]
Robot Arm 1 end: [-5.48692287955391;-0.826591649894446;0.244605913699918]
Robot Arm 1 togoal: 18.9390878580347
Mobile Platform 2 Pose: [9.9634134611079;-2.75563229146808;-2.65864835915519]
Robot Arm 2 end: [-2.40268776180616;-2.45202177746824;0.486797406265228]
Robot Arm 1 togoal: 10.7592148178811
Step 144:
Mobile Platform 1 Pose: [-2.15022638637884;-5.63329889508161;-1.29341475504894]
Robot Arm 1 end: [-5.43424606913277;-0.691517931191221;0.157365816897499]
Robot Arm 1 togoal: 18.8250897894499
Mobile Platform 2 Pose: [10.1034665684994;-2.83526648290641;-2.65414836256784]
Robot Arm 2 end: [-2.49815525537607;-2.48708598951905;0.41192153745103]
Robot Arm 1 togoal: 10.6748117799525
Step 145:
Mobile Platform 1 Pose: [-1.89363461623734;-5.71359010321251;-1.14891162530344]
Robot Arm 1 end: [-5.39749185638379;-0.556639518040469;0.248146806092834]
Robot Arm 1 togoal: 18.7121764804231
Mobile Platform 2 Pose: [10.2564662490927;-2.92079258870729;-2.64619514987054]
Robot Arm 2 end: [-2.61410326374124;-2.51928084298639;0.346056979995858]
Robot Arm 1 togoal: 10.5779544716852
Step 146:
Mobile Platform 1 Pose: [-2.12979532052624;-5.55047377609143;-1.03243153354684]
Robot Arm 1 end: [-5.34800311754428;-0.4274315406291;0.179830020540386]
Robot Arm 1 togoal: 18.603528298268
Mobile Platform 2 Pose: [10.4326351616456;-3.01608993841804;-2.63212831042454]
Robot Arm 2 end: [-2.75452095321561;-2.53181176119313;0.28689260735276]
Robot Arm 1 togoal: 10.4782547804599
Step 147:
Mobile Platform 1 Pose: [-1.92983505679729;-5.59109186431912;-0.859726957376705]
Robot Arm 1 end: [-5.28867770845445;-0.292350045351361;0.207326131545161]
Robot Arm 1 togoal: 18.4770634723621
Mobile Platform 2 Pose: [10.6418073900422;-3.12471734426185;-2.60788652862089]
Robot Arm 2 end: [-2.90271255141019;-2.53739369208124;0.202470363680815]
Robot Arm 1 togoal: 10.3827020778192
Step 148:
Mobile Platform 1 Pose: [-2.16579602167557;-5.42751995203827;-0.740941785875647]
Robot Arm 1 end: [-5.23121893697075;-0.145850934897696;0.164273805603205]
Robot Arm 1 togoal: 18.3512677345644
Mobile Platform 2 Pose: [10.8764445839862;-3.23896449800904;-2.56845006586093]
Robot Arm 2 end: [-3.02763380316243;-2.53341927972542;0.131551654307077]
Robot Arm 1 togoal: 10.3099119657021
Step 149:
Mobile Platform 1 Pose: [-1.9464112675254;-5.48159479112839;-0.578497316937834]
Robot Arm 1 end: [-5.18128679363262;-0.0128532852921742;0.210129280187054]
Robot Arm 1 togoal: 18.2331314279082
Mobile Platform 2 Pose: [11.0677672677269;-3.3264910418222;-2.52943029993784]
Robot Arm 2 end: [-3.14475061436185;-2.51972752837289;0.074782092687777]
Robot Arm 1 togoal: 10.2489251448386
Step 150:
Mobile Platform 1 Pose: [-2.17293765067114;-5.32214336010519;-0.457953644891289]
Robot Arm 1 end: [-5.12660072098319;0.137469452690462;0.164666279955195]
Robot Arm 1 togoal: 18.1086498483126
Mobile Platform 2 Pose: [11.1730281226791;-3.38305930239593;-2.50841571181034]
Robot Arm 2 end: [-3.1979782396375;-2.53339855151006;0.0441586122151402]
Robot Arm 1 togoal: 10.207776334918
Step 151:
Mobile Platform 1 Pose: [-1.95197202245077;-5.37803414535314;-0.298892849613743]
Robot Arm 1 end: [-5.07877231540597;0.266431517202584;0.210989022871364]
Robot Arm 1 togoal: 17.9952176621834
Mobile Platform 2 Pose: [11.2788451673692;-3.4454250470817;-2.48898541079946]
Robot Arm 2 end: [-3.21871374589372;-2.54660498933361;0.0372500349230565]
Robot Arm 1 togoal: 10.1853058080758
Step 152:
Mobile Platform 1 Pose: [-2.17386042262974;-5.22063202544255;-0.17778881599885]
Robot Arm 1 end: [-5.02509186433595;0.418322378085915;0.164262014611154]
Robot Arm 1 togoal: 17.8718401275028
Mobile Platform 2 Pose: [11.3984339809553;-3.51931906289122;-2.46939003804264]
Robot Arm 2 end: [-3.25791612064103;-2.57063631093526;0.0395809990310937]
Robot Arm 1 togoal: 10.1412889771043
Step 153:
Mobile Platform 1 Pose: [-1.94958473670677;-5.27936265975229;-0.0220111018591442]
Robot Arm 1 end: [-4.98033469058752;0.543463896856904;0.210833131617474]
Robot Arm 1 togoal: 17.7638126463627
Mobile Platform 2 Pose: [11.5235355417709;-3.60184243933978;-2.4508675508575]
Robot Arm 2 end: [-3.31499802393138;-2.61094134391372;0.0501148098093491]
Robot Arm 1 togoal: 10.0722858620001
Step 154:
Mobile Platform 1 Pose: [-2.16982187820215;-5.12292972243095;0.0986696760637003]
Robot Arm 1 end: [-4.92697617484664;0.695122079376499;0.163861418281998]
Robot Arm 1 togoal: 17.6419158815627
Mobile Platform 2 Pose: [11.6438135799448;-3.69011751765587;-2.43375218740941]
Robot Arm 2 end: [-3.38784585101681;-2.66391574374403;0.0691353763174206]
Robot Arm 1 togoal: 9.98231564269049
Step 155:
Mobile Platform 1 Pose: [-1.94501653659673;-5.18246147341564;0.253093525770346]
Robot Arm 1 end: [-4.8831593535349;0.818958390848203;0.210058878668227]
Robot Arm 1 togoal: 17.5362060889831
Mobile Platform 2 Pose: [11.7448736043501;-3.78085101865342;-2.41889545766612]
Robot Arm 2 end: [-3.46990231268409;-2.72141395792267;0.0897811566273645]
Robot Arm 1 togoal: 9.88273130037625
Step 156:
Mobile Platform 1 Pose: [-2.16538797661382;-5.02636645444142;0.372765916635288]
Robot Arm 1 end: [-4.82984992434103;0.969378866660969;0.163517167699413]
Robot Arm 1 togoal: 17.4160651833983
Mobile Platform 2 Pose: [11.8330333254126;-3.88124202252798;-2.3987535587328]
Robot Arm 2 end: [-3.557468358269;-2.76228116303766;0.105176438014605]
Robot Arm 1 togoal: 9.79261682474596
Step 157:
Mobile Platform 1 Pose: [-1.94140962034078;-5.08570252462605;0.526729586044898]
Robot Arm 1 end: [-4.78593365996013;1.093067262384;0.209078825762767]
Robot Arm 1 togoal: 17.3112224339175
Mobile Platform 2 Pose: [11.95803199991;-4.01253309301754;-2.34867239111491]
Robot Arm 2 end: [-3.66835701082631;-2.76091297883153;0.129161014652343]
Robot Arm 1 togoal: 9.71757683805668
Step 158:
Mobile Platform 1 Pose: [-2.16252284594161;-4.92975846109123;0.645097148923193]
Robot Arm 1 end: [-4.73259378754177;1.24178878797528;0.163203506913038]
Robot Arm 1 togoal: 17.1929984196995
Mobile Platform 2 Pose: [12.0818747660872;-4.16077904552785;-2.26952483802978]
Robot Arm 2 end: [-3.81282783651948;-2.73939546503482;0.163026646707775]
Robot Arm 1 togoal: 9.63543300645886
Step 159:
Mobile Platform 1 Pose: [-1.94002341129558;-4.9884181524361;0.799020802185769]
Robot Arm 1 end: [-4.68808282030325;1.36587497283742;0.208008579708764]
Robot Arm 1 togoal: 17.0883538818261
Mobile Platform 2 Pose: [12.0208171717369;-4.29422301191616;-2.12423713744096]
Robot Arm 2 end: [-4.02414283587654;-2.68926278972867;0.204340094951346]
Robot Arm 1 togoal: 9.53369501868055
Step 160:
Mobile Platform 1 Pose: [-2.16212947742066;-4.83257209095474;0.915870567941764]
Robot Arm 1 end: [-4.63467969082549;1.51259251469092;0.162906140660783]
Robot Arm 1 togoal: 16.9721641049548
Mobile Platform 2 Pose: [11.5859352613546;-4.27239994793539;-1.99401299614681]
Robot Arm 2 end: [-4.09967033914043;-2.65624334208346;0.308626795092258]
Robot Arm 1 togoal: 9.49813143441861
Step 161:
Mobile Platform 1 Pose: [-1.94154653856313;-4.89023066238832;1.07003619431148]
Robot Arm 1 end: [-4.58924781211029;1.6374442869455;0.20687113137557]
Robot Arm 1 togoal: 16.8672885924341
Mobile Platform 2 Pose: [11.4598582352374;-4.51840596725668;-2.0385218543602]
Robot Arm 2 end: [-4.22888385838396;-2.62907530581202;0.248949554828763]
Robot Arm 1 togoal: 9.44745599103122
Step 162:
Mobile Platform 1 Pose: [-2.16479984120311;-4.73447029717296;1.18516511835263]
Robot Arm 1 end: [-4.53575824820221;1.78187509363262;0.162620760608189]
Robot Arm 1 togoal: 16.7532396564021
Mobile Platform 2 Pose: [11.4926917305054;-4.41024397900851;-1.81313111742053]
Robot Arm 2 end: [-4.23181322442606;-2.49181213117962;0.292577197610496]
Robot Arm 1 togoal: 9.54745444256416
Step 163:
Mobile Platform 1 Pose: [-1.94656072000214;-4.79080895932292;1.33985090823059]
Robot Arm 1 end: [-4.48909239170297;1.90785475384553;0.205661534699916]
Robot Arm 1 togoal: 16.6477244180237
Mobile Platform 2 Pose: [11.8702981164239;-4.60812329565444;-1.91303953798496]
Robot Arm 2 end: [-4.54728942202814;-2.60024400978085;0.212365870617707]
Robot Arm 1 togoal: 9.28444746048436
Step 164:
Mobile Platform 1 Pose: [-2.17109393782663;-4.63513923795024;1.45303396752414]
Robot Arm 1 end: [-4.43549423723414;2.04968480612813;0.162347501935095]
Robot Arm 1 togoal: 16.5359288268191
Mobile Platform 2 Pose: [11.910822995457;-4.63291347845703;-1.9194035054278]
Robot Arm 2 end: [-4.70267367846358;-2.61277261453216;0.198610221312315]
Robot Arm 1 togoal: 9.18594085155278
Step 165:
Mobile Platform 1 Pose: [-1.95571520806158;-4.68977316651538;1.608575016781]
Robot Arm 1 end: [-4.38722110164603;2.17722691459814;0.204363115084878]
Robot Arm 1 togoal: 16.4292861214374
Mobile Platform 2 Pose: [11.9622139082545;-4.66496879054353;-1.92499871954306]
Robot Arm 2 end: [-4.86568801653938;-2.59431874962013;0.195766545721605]
Robot Arm 1 togoal: 9.1083213620334
Step 166:
Mobile Platform 1 Pose: [-2.18166026603967;-4.53421351194704;1.71954478295279]
Robot Arm 1 end: [-4.33349153310906;2.31607909621693;0.162087977105805]
Robot Arm 1 togoal: 16.3198732186028
Mobile Platform 2 Pose: [12.0373552778394;-4.7115034814952;-1.93167820669639]
Robot Arm 2 end: [-5.03401272947035;-2.54908519926721;0.195084604815473]
Robot Arm 1 togoal: 9.05180714410271
Step 167:
Mobile Platform 1 Pose: [-1.96982875784569;-4.58663523844083;1.8763760544726]
Robot Arm 1 end: [-4.28312200503154;2.44574568299479;0.202949613000844]
Robot Arm 1 togoal: 16.2114647493457
Mobile Platform 2 Pose: [12.1084120512335;-4.75778566468009;-1.93691096195566]
Robot Arm 2 end: [-5.18599337052823;-2.48275265488645;0.193549211436796]
Robot Arm 1 togoal: 9.02473229920267
Step 168:
Mobile Platform 1 Pose: [-2.19731972660825;-4.43122524898881;1.98480174653081]
Robot Arm 1 end: [-4.22923799378818;2.58114830150464;0.161843660147934]
Robot Arm 1 togoal: 16.1045919030658
Mobile Platform 2 Pose: [12.1663949555369;-4.79819876070942;-1.93985066557002]
Robot Arm 2 end: [-5.31501354077533;-2.39530653406194;0.192785202744941]
Robot Arm 1 togoal: 9.03020572200029
Step 169:
Mobile Platform 1 Pose: [-1.99000167708498;-4.4807311164543;2.14350758479338]
Robot Arm 1 end: [-4.17609705539531;2.71369273345628;0.201382315066477]
Robot Arm 1 togoal: 15.9935422011781
Mobile Platform 2 Pose: [12.2164592667252;-4.83552709095218;-1.9408421827437]
Robot Arm 2 end: [-5.42055768756878;-2.29097891648891;0.192643356492799]
Robot Arm 1 togoal: 9.06448174642398
Step 170:
Mobile Platform 1 Pose: [-2.21916543368145;-4.32554434709822;2.24896756149135]
Robot Arm 1 end: [-4.12203748867308;2.8450385957991;0.161614174039021]
Robot Arm 1 togoal: 15.8894098490446
Mobile Platform 2 Pose: [12.2638759217828;-4.8729182947073;-1.93994568644494]
Robot Arm 2 end: [-5.50326102726712;-2.17456380255899;0.19341903258788]
Robot Arm 1 togoal: 9.12253168227711
Step 171:
Mobile Platform 1 Pose: [-2.01779118579422;-4.37111401877774;2.41035335684538]
Robot Arm 1 end: [-4.06515156973007;2.98149809504112;0.199603850178865]
Robot Arm 1 togoal: 15.7744713209081
Mobile Platform 2 Pose: [12.31288524044;-4.91310340642889;-1.93682178036489]
Robot Arm 2 end: [-5.56438552000642;-2.0506436312164;0.196382600185594]
Robot Arm 1 togoal: 9.1989974317281
Step 172:
Mobile Platform 1 Pose: [-2.2487178704702;-4.21628529239483;2.51229720279246]
Robot Arm 1 end: [-4.01259486818135;3.11064754952544;0.163424508134122]
Robot Arm 1 togoal: 15.6735127952465
Mobile Platform 2 Pose: [12.368769091491;-4.95971668369631;-1.93064350944985]
Robot Arm 2 end: [-5.61773174860078;-1.92265265266279;0.220121575494799]
Robot Arm 1 togoal: 9.2811691005013
Step 173:
Mobile Platform 1 Pose: [-2.05144874450495;-4.25932006513622;2.6766274998484]
Robot Arm 1 end: [-3.95376728782636;3.25185246668875;0.199016608693181]
Robot Arm 1 togoal: 15.5561163772016
Mobile Platform 2 Pose: [12.4453070578423;-5.02332646892889;-1.91892891757169]
Robot Arm 2 end: [-5.70007768381537;-1.78127383489617;0.312888247484649]
Robot Arm 1 togoal: 9.35394706574452
Step 174:
Mobile Platform 1 Pose: [-2.28160712762587;-4.10661637970576;2.77472705311661]
Robot Arm 1 end: [-3.90005693088907;3.37355312038063;0.162240023328237]
Robot Arm 1 togoal: 15.4585772179906
Mobile Platform 2 Pose: [12.5798677306991;-5.14411928663385;-1.88627884532743]
Robot Arm 2 end: [-5.82626061739638;-1.61134159863755;0.435864207896946]
Robot Arm 1 togoal: 9.43224309991787
Step 175:
Mobile Platform 1 Pose: [-2.09065790330333;-4.14498258492033;2.94341895118397]
Robot Arm 1 end: [-3.83746167417894;3.52109015727896;0.19600942087553]
Robot Arm 1 togoal: 15.336466945462
Mobile Platform 2 Pose: [12.7119430703619;-5.30792609598425;-1.80895497837263]
Robot Arm 2 end: [-5.94908377708617;-1.40118628416639;0.571407809502599]
Robot Arm 1 togoal: 9.55255174772313
Step 176:
Mobile Platform 1 Pose: [-2.32324626804094;-3.99257393422117;3.03687667014183]
Robot Arm 1 end: [-3.78657457740977;3.64125513824308;0.165163566845308]
Robot Arm 1 togoal: 15.2427545814026
Mobile Platform 2 Pose: [12.6071185561169;-5.37665458546082;-1.67940981030743]
Robot Arm 2 end: [-5.91859926155815;-1.1760473324771;0.461666476004636]
Robot Arm 1 togoal: 9.77968098567119
Step 177:
Mobile Platform 1 Pose: [-2.13466619372295;-4.02967046280169;3.20939650744322]
Robot Arm 1 end: [-3.72365418171081;3.79503032283039;0.195191664315053]
Robot Arm 1 togoal: 15.1194609869782
Mobile Platform 2 Pose: [12.7676932008754;-5.55552479248755;-1.56708796425647]
Robot Arm 2 end: [-6.00337426681142;-0.946368334342917;0.659662459749692]
Robot Arm 1 togoal: 9.93394010183363

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.925489e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-2.36489602345343;-3.87927616527937;3.30166208422677]
Robot Arm 1 end: [-3.66812884284873;3.90861022969778;0.160597829704345]
Robot Arm 1 togoal: 15.025760195857
Mobile Platform 2 Pose: [12.4782128428902;-5.45848055987186;-1.4731812774008]
Robot Arm 2 end: [-5.92829989610297;-0.875848744071548;0.449274772768925]
Robot Arm 1 togoal: 10.0487547905111
Step 179:
Mobile Platform 1 Pose: [-2.17183163366489;-3.91824371938165;3.47469795678583]
Robot Arm 1 end: [-3.60970099644984;4.06460489820556;0.191679982308057]
Robot Arm 1 togoal: 14.9070442387688
Mobile Platform 2 Pose: [12.6281279451525;-5.62317701208963;-1.36158532279868]
Robot Arm 2 end: [-5.96833002259909;-0.678407886171813;0.659662459756753]
Robot Arm 1 togoal: 10.1925632056658
Step 180:
Mobile Platform 1 Pose: [-2.40242592995521;-3.76960082376339;3.56099807997827]
Robot Arm 1 end: [-3.5557841780792;4.17312256970769;0.162671555522796]
Robot Arm 1 togoal: 14.8174505713158
Mobile Platform 2 Pose: [12.3250066150676;-5.50439821378968;-1.27435525516931]
Robot Arm 2 end: [-5.90999230169634;-0.63153868180913;0.449756037806693]
Robot Arm 1 togoal: 10.2783166943404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.109378e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-2.23133961564324;-3.79410209965244;3.74507406211401]
Robot Arm 1 end: [-3.39215384770591;4.20462932661484;0.106852028313536]
Robot Arm 1 togoal: 14.6606930398389
Mobile Platform 2 Pose: [12.4145021192261;-5.60948580353855;-1.1810681135954]
Robot Arm 2 end: [-5.87995736238901;-0.46503239765469;0.659662459738098]
Robot Arm 1 togoal: 10.4226801194209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.230262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-2.51347711176624;-3.60818212982139;3.82122498963392]
Robot Arm 1 end: [-3.46690149834672;4.35750332761594;0.0318245719901082]
Robot Arm 1 togoal: 14.6769495508919
Mobile Platform 2 Pose: [12.1066579210899;-5.4794585910747;-1.09905151573602]
Robot Arm 2 end: [-5.82120060563879;-0.420131707092578;0.449756037654691]
Robot Arm 1 togoal: 10.5063697114954

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.432181e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-2.23295436887694;-3.7182393897453;3.94802425918399]
Robot Arm 1 end: [-3.38299534773138;4.47805306309739;0.113929672401834]
Robot Arm 1 togoal: 14.5456629054222
Mobile Platform 2 Pose: [12.1917017790997;-5.57956367142207;-0.998958776970499]
Robot Arm 2 end: [-5.7796971314955;-0.260170168048926;0.659662459407359]
Robot Arm 1 togoal: 10.6497485329986
Step 184:
Mobile Platform 1 Pose: [-2.49730515555823;-3.54836600325107;4.02476593433225]
Robot Arm 1 end: [-3.36115781523043;4.66475978612171;0.174752222735333]
Robot Arm 1 togoal: 14.4498342336882
Mobile Platform 2 Pose: [11.881747111376;-5.44120554209035;-0.921990902684755]
Robot Arm 2 end: [-5.71990067700472;-0.214849663266266;0.449756037193056]
Robot Arm 1 togoal: 10.7338707349695
Step 185:
Mobile Platform 1 Pose: [-2.4140305196489;-3.53169913446959;4.22624129674175]
Robot Arm 1 end: [-3.18537662968028;4.78535439121882;0.137176026800214]
Robot Arm 1 togoal: 14.2464611212926
Mobile Platform 2 Pose: [11.9615612235949;-5.5358315928192;-0.814958646217805]
Robot Arm 2 end: [-5.66799894380413;-0.061039683786589;0.659662458635485]
Robot Arm 1 togoal: 10.8761697154058
Step 186:
Mobile Platform 1 Pose: [-2.6696789970541;-3.35621081138622;4.32807538990798]
Robot Arm 1 end: [-3.15926022476211;4.97859712620169;0.170205990187151]
Robot Arm 1 togoal: 14.1493908328572
Mobile Platform 2 Pose: [11.6510146486963;-5.39099323836847;-0.742890453131374]
Robot Arm 2 end: [-5.6065082108233;-0.0140056835216589;0.44975603691685]
Robot Arm 1 togoal: 10.962226791065
Step 187:
Mobile Platform 1 Pose: [-2.66869426326834;-3.29585556420365;4.53105527565097]
Robot Arm 1 end: [-3.09804242447207;5.16734738271584;0.134628482940729]
Robot Arm 1 togoal: 14.0297994517358
Mobile Platform 2 Pose: [11.7241193996539;-5.47924887199215;-0.628348139448484]
Robot Arm 2 end: [-5.54460476359829;0.134272747942876;0.659662458823503]
Robot Arm 1 togoal: 11.1038701902575

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.323028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.974205e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-2.68976555126727;-3.23406076986794;4.71065565497687]
Robot Arm 1 end: [-3.04389499829686;5.30190492200747;0.0865831913532341]
Robot Arm 1 togoal: 13.9381794244937
Mobile Platform 2 Pose: [11.4139373980409;-5.3292020588314;-0.561055507344001]
Robot Arm 2 end: [-5.48074157361013;0.183763588849492;0.44975603695242]
Robot Arm 1 togoal: 11.192868318105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.596896e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-2.8737515046277;-3.09780590882753;4.83165193310361]
Robot Arm 1 end: [-2.96430485531169;5.53364848838065;0.182744682838041]
Robot Arm 1 togoal: 13.7772429571636
Mobile Platform 2 Pose: [11.4778040786729;-5.40953530251996;-0.438084008624879]
Robot Arm 2 end: [-5.40843367413585;0.327331432175305;0.659662458803763]
Robot Arm 1 togoal: 11.3348206070797

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.008265e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-2.75599907190818;-3.15005764019328;4.85868590935787]
Robot Arm 1 end: [-2.85874090308116;5.55928434679501;0.30329905112295]
Robot Arm 1 togoal: 13.6583296462667
Mobile Platform 2 Pose: [11.1686301360529;-5.25520956353006;-0.375466071258262]
Robot Arm 2 end: [-5.34149881897208;0.379698728674913;0.449756036928529]
Robot Arm 1 togoal: 11.4274819002312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.554404e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-2.55656960252173;-3.2226687781207;4.93520256808134]
Robot Arm 1 end: [-2.78043660939485;5.57403715047907;0.426998223283241]
Robot Arm 1 togoal: 13.569286899449
Mobile Platform 2 Pose: [11.2191482604805;-5.3250327410801;-0.242899890329444]
Robot Arm 2 end: [-5.25726957462454;0.519708922920221;0.630803582701185]
Robot Arm 1 togoal: 11.5736864210208

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.475201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-2.71211648840617;-3.07049637016308;5.08130605125929]
Robot Arm 1 end: [-2.80211427365167;5.79902451054073;0.328462912114733]
Robot Arm 1 togoal: 13.5264359549977
Mobile Platform 2 Pose: [10.8912895780776;-5.15985573977255;-0.167000260523034]
Robot Arm 2 end: [-5.16378975024393;0.585363466708325;0.421263122589548]
Robot Arm 1 togoal: 11.6896651150367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.139450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-2.5259076693424;-3.1412577526171;5.14982175916987]
Robot Arm 1 end: [-2.70716198669752;5.79055852745556;0.428993100498816]
Robot Arm 1 togoal: 13.4307052061873
Mobile Platform 2 Pose: [11.0236834455207;-5.25831719855041;-0.0659189407244354]
Robot Arm 2 end: [-5.10910804350674;0.685014263179332;0.630803581460384]
Robot Arm 1 togoal: 11.7848548399213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.858904e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-2.62160296698732;-3.01403648874372;5.30813649681649]
Robot Arm 1 end: [-2.6871741695863;5.9905553519891;0.33193419418681]
Robot Arm 1 togoal: 13.3586643151715
Mobile Platform 2 Pose: [10.6903210504811;-5.08399044535091;0.00399929643913895]
Robot Arm 2 end: [-5.01238716140316;0.75295832320163;0.421263121750423]
Robot Arm 1 togoal: 11.9042772031581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.574217e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-2.43172359605249;-3.07309872217549;5.40597623712134]
Robot Arm 1 end: [-2.58991841662094;5.99758609144226;0.432627764289411]
Robot Arm 1 togoal: 13.2555617367222
Mobile Platform 2 Pose: [10.8262437055032;-5.18150058163494;0.118917055291136]
Robot Arm 2 end: [-4.95771621275659;0.851095042611692;0.630803581458413]
Robot Arm 1 togoal: 11.9984723643668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.510176e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-2.59747103702753;-2.92026972222761;5.53876403534844]
Robot Arm 1 end: [-2.59559142997115;6.22326636219176;0.334443626927734]
Robot Arm 1 togoal: 13.2030510287658
Mobile Platform 2 Pose: [10.4893638441885;-5.00003908891261;0.18297910992206]
Robot Arm 2 end: [-4.85683612535724;0.921599151644624;0.421263122646128]
Robot Arm 1 togoal: 12.1219999946398

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.428640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-2.41719105497807;-2.98510797295157;5.62200423575787]
Robot Arm 1 end: [-2.50568750197144;6.21725705081057;0.43416365794741]
Robot Arm 1 togoal: 13.1103995877705
Mobile Platform 2 Pose: [10.622927098964;-5.09269168667922;0.315192296262893]
Robot Arm 2 end: [-4.79959365319205;1.02248119888138;0.630803582096496]
Robot Arm 1 togoal: 12.2201377903059

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.606362e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-2.48194238288279;-2.88785090629904;5.76689609839943]
Robot Arm 1 end: [-2.45490641286627;6.37413612973146;0.339152509492847]
Robot Arm 1 togoal: 13.0256802528618
Mobile Platform 2 Pose: [10.2841535129094;-4.90568718310134;0.373536579922261]
Robot Arm 2 end: [-4.69363960203258;1.09541086162787;0.421263123394828]
Robot Arm 1 togoal: 12.3480777475367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.558790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-2.3133359684805;-2.93445995685695;5.8725230689713]
Robot Arm 1 end: [-2.3833701552643;6.38570792603141;0.441346870873047]
Robot Arm 1 togoal: 12.9451386426518
Mobile Platform 2 Pose: [10.4037946673063;-4.98577549962158;0.526912197356007]
Robot Arm 2 end: [-4.62728032680079;1.20340703111692;0.63080358285506]
Robot Arm 1 togoal: 12.4569215881276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.473413e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-2.24323663446662;-2.90561021367233;6.02146649853503]
Robot Arm 1 end: [-2.30305937197542;6.48841704232533;0.568574697287914]
Robot Arm 1 togoal: 12.8297934019309
Mobile Platform 2 Pose: [10.0645333424088;-4.79452796630451;0.579656515074211]
Robot Arm 2 end: [-4.51540803131029;1.27834007509889;0.42126312316974]
Robot Arm 1 togoal: 12.5893258402052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.184040e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-2.43838321390614;-2.7536948703763;6.11549659317195]
Robot Arm 1 end: [-2.21559107527505;6.61812535959423;0.361741327172927]
Robot Arm 1 togoal: 12.7279857822157
Mobile Platform 2 Pose: [10.1493480330442;-4.84826279412513;0.757414078856218]
Robot Arm 2 end: [-4.43148090545734;1.39071039321594;0.532868032589066]
Robot Arm 1 togoal: 12.7174326400191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.610577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-2.61666179831951;-2.61338122078603;6.21752322155199]
Robot Arm 1 end: [-2.224613036834;6.82308833452604;0.261530843676135]
Robot Arm 1 togoal: 12.6933129712572
Mobile Platform 2 Pose: [9.88106474605923;-4.68770067445283;0.949038205020532]
Robot Arm 2 end: [-4.15079673450534;1.51880457837039;0.608173670473376]
Robot Arm 1 togoal: 12.9510343086426

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-2.49319806368767;-2.64204613363955;6.32983382878025]
Robot Arm 1 end: [-2.12504792125596;6.87418159998351;0.357922483723525]
Robot Arm 1 togoal: 12.5753837743229
Mobile Platform 2 Pose: [9.62834652200502;-4.54069522393942;0.967755446668918]
Robot Arm 2 end: [-4.0472429078178;1.56246462399417;0.46646094402778]
Robot Arm 1 togoal: 13.0475268844411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-2.51701852997851;-2.59015642339341;6.43861315262884]
Robot Arm 1 end: [-2.1447702844634;7.00370487439906;0.267082808311166]
Robot Arm 1 togoal: 12.5716163664701
Mobile Platform 2 Pose: [9.32067443587766;-4.36083171364613;1.00748402559673]
Robot Arm 2 end: [-3.94639864390644;1.62800892989242;0.330396561086188]
Robot Arm 1 togoal: 13.1633664995285

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-2.3643925079466;-2.64909496523712;6.50769817743421]
Robot Arm 1 end: [-2.09982311649996;7.00331992918635;0.366604150385293]
Robot Arm 1 togoal: 12.5187236040663
Mobile Platform 2 Pose: [9.00777804420317;-4.17848569822546;1.07817522829588]
Robot Arm 2 end: [-3.84990921297529;1.68675680704799;0.18817584338928]
Robot Arm 1 togoal: 13.2732935471259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-2.20602671450654;-2.70612095894701;6.58266016913237]
Robot Arm 1 end: [-2.10783052462917;7.05694087198387;0.565718796669407]
Robot Arm 1 togoal: 12.4969483963717
Mobile Platform 2 Pose: [8.77667236078159;-4.04280639559398;1.19432173893679]
Robot Arm 2 end: [-3.65718010345848;1.78174656549178;0.187372272397603]
Robot Arm 1 togoal: 13.4469133740524

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-2.31245921965086;-2.61511638755992;6.66576523098102]
Robot Arm 1 end: [-2.09801005743989;7.19242180300573;0.362697343466997]
Robot Arm 1 togoal: 12.4734120322503
Mobile Platform 2 Pose: [8.54558050935124;-3.91045140736676;1.22149717744847]
Robot Arm 2 end: [-3.54795381154115;1.80273341817244;0.148227066452128]
Robot Arm 1 togoal: 13.5210177755054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.265363e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009237e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-2.22595387762756;-2.62997239536561;6.76037603085938]
Robot Arm 1 end: [-2.05834616979504;7.2659356925598;0.486317900842443]
Robot Arm 1 togoal: 12.4076342239021
Mobile Platform 2 Pose: [8.34177848906457;-3.79354116886127;1.24404239230084]
Robot Arm 2 end: [-3.47405092372571;1.84086436868664;0.127854408444782]
Robot Arm 1 togoal: 13.5917230321629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.940728e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012105e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-2.31579331226424;-2.55622762967999;6.82835413260129]
Robot Arm 1 end: [-2.08198713504274;7.37028236247949;0.291508076798594]
Robot Arm 1 togoal: 12.4258391416291
Mobile Platform 2 Pose: [8.13959379374489;-3.6792006761692;1.26330549210616]
Robot Arm 2 end: [-3.40442841103021;1.86732326987552;0.114995128588997]
Robot Arm 1 togoal: 13.64960716927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-2.18877269187158;-2.59921384722801;6.90799661862783]
Robot Arm 1 end: [-2.02335415966449;7.41454474482089;0.482179903024714]
Robot Arm 1 togoal: 12.3419921940963
Mobile Platform 2 Pose: [7.91992475922623;-3.55790991583107;1.28777296836653]
Robot Arm 2 end: [-3.38727443552913;1.86110248551926;0.0542209779421924]
Robot Arm 1 togoal: 13.6588920583826

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012676e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-2.28880593090195;-2.52862651564531;6.95226672409932]
Robot Arm 1 end: [-2.05016724366691;7.49580172407533;0.289373271090327]
Robot Arm 1 togoal: 12.3690922870214
Mobile Platform 2 Pose: [7.93579249180302;-3.56872298767868;1.32619852410625]
Robot Arm 2 end: [-3.25616897182825;1.90848849116794;0.143011853519341]
Robot Arm 1 togoal: 13.7546430856149
Step 212:
Mobile Platform 1 Pose: [-2.20975920958666;-2.53336319797887;7.06000136534005]
Robot Arm 1 end: [-1.96770477640232;7.55655745706318;0.385591403551458]
Robot Arm 1 togoal: 12.2672540430467
Mobile Platform 2 Pose: [8.01512531084824;-3.60772261239028;1.42707146261641]
Robot Arm 2 end: [-3.17207515333972;1.95149391146552;0.202906047212466]
Robot Arm 1 togoal: 13.8273690952333
Step 213:
Mobile Platform 1 Pose: [-2.32877849195393;-2.43668176600011;7.13872706355735]
Robot Arm 1 end: [-1.98037416221062;7.65283468521348;0.200677035908968]
Robot Arm 1 togoal: 12.2793242000937
Mobile Platform 2 Pose: [8.22100328145645;-3.71259056230495;1.50131598438805]
Robot Arm 2 end: [-3.1836875601168;1.95576820883667;0.290304606887971]
Robot Arm 1 togoal: 13.8172761386665
Step 214:
Mobile Platform 1 Pose: [-2.17986347552051;-2.48205025670926;7.23542495033602]
Robot Arm 1 end: [-1.91426023673326;7.72815922555071;0.382535715421022]
Robot Arm 1 togoal: 12.1822394665478
Mobile Platform 2 Pose: [8.31990057091526;-3.76816239433473;1.50646342243874]
Robot Arm 2 end: [-3.12231818249837;1.87060878731359;0.460469654867961]
Robot Arm 1 togoal: 13.7600357840082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.825670e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-2.30094938604131;-2.39916578664745;7.27980394572686]
Robot Arm 1 end: [-1.93936817791784;7.79032593827844;0.200179581901946]
Robot Arm 1 togoal: 12.2137520248197
Mobile Platform 2 Pose: [8.79010846610478;-4.0024541685933;1.59368363186665]
Robot Arm 2 end: [-3.2904759488413;1.86783335950893;0.610605891219671]
Robot Arm 1 togoal: 13.6635280610629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.111134e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-2.26602594538894;-2.36687817756169;7.41461296353962]
Robot Arm 1 end: [-1.85523346621197;7.86104151067299;0.191547012937298]
Robot Arm 1 togoal: 12.119783380996
Mobile Platform 2 Pose: [8.40661749541581;-3.75597036572776;1.63546562518425]
Robot Arm 2 end: [-3.12079829349379;2.02670104282198;0.410972993376816]
Robot Arm 1 togoal: 13.8995364688052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-2.30772998649736;-2.28458610167143;7.55138078863067]
Robot Arm 1 end: [-1.82875718510734;8.06162212085664;0.165475225742575]
Robot Arm 1 togoal: 12.0629288632656
Mobile Platform 2 Pose: [8.17201931764144;-3.56572418294783;1.79765807241282]
Robot Arm 2 end: [-2.97918822316951;2.18901221755399;0.3444788276341]
Robot Arm 1 togoal: 14.1155240396659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-2.26302373528078;-2.27810751930287;7.64329579669538]
Robot Arm 1 end: [-1.78176591891144;8.14380942469824;0.203242497889308]
Robot Arm 1 togoal: 11.9996640142227
Mobile Platform 2 Pose: [8.26035621075354;-3.61734189310987;1.79788077739038]
Robot Arm 2 end: [-2.93253413295966;2.09140149053063;0.471411372775737]
Robot Arm 1 togoal: 14.0446680300171

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-2.2865261771022;-2.23409896319334;7.72904790169379]
Robot Arm 1 end: [-1.80205875061904;8.28994912647633;0.191529383686409]
Robot Arm 1 togoal: 11.9991815080339
Mobile Platform 2 Pose: [8.6395535831394;-3.73519164307518;2.01836079107215]
Robot Arm 2 end: [-3.03382984934235;2.28857308806178;0.553903178157735]
Robot Arm 1 togoal: 14.1588075547119

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-2.21300300653686;-2.25200594413961;7.7972776042979]
Robot Arm 1 end: [-1.722251741714;8.33821741204244;0.301874832773713]
Robot Arm 1 togoal: 11.9020575296509
Mobile Platform 2 Pose: [8.38869019395179;-3.52312044496413;2.15074586172614]
Robot Arm 2 end: [-2.8824002955966;2.53100073372623;0.471411372737579]
Robot Arm 1 togoal: 14.4494927671268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-2.29217483421571;-2.18723638847015;7.85599868100326]
Robot Arm 1 end: [-1.76386675920829;8.43932465180433;0.207530044095903]
Robot Arm 1 togoal: 11.9394083603809
Mobile Platform 2 Pose: [8.85464887894824;-3.72132876398371;2.30310550950667]
Robot Arm 2 end: [-3.02323852993502;2.5825189897871;0.556380583326151]
Robot Arm 1 togoal: 14.4196211932755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-2.20192083074727;-2.20500199990863;7.94048177638205]
Robot Arm 1 end: [-1.65951093011485;8.49360075598882;0.357544029103457]
Robot Arm 1 togoal: 11.8138504988018
Mobile Platform 2 Pose: [8.66990582446353;-3.53353581096559;2.46813219461744]
Robot Arm 2 end: [-2.88950736100447;2.8427565840077;0.471411372737579]
Robot Arm 1 togoal: 14.7172395010668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.233048e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-2.31791585912722;-2.12116289797826;7.99263439252788]
Robot Arm 1 end: [-1.69005654887217;8.5870008458387;0.229789969897504]
Robot Arm 1 togoal: 11.8457254766151
Mobile Platform 2 Pose: [9.23913292016224;-3.80459107808534;2.64118608971977]
Robot Arm 2 end: [-3.10480576333908;2.91641087649265;0.556647675672474]
Robot Arm 1 togoal: 14.6733550661427

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.107365e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-2.24742533807198;-2.11481798939057;8.09954880410736]
Robot Arm 1 end: [-1.56691492712532;8.65091249198635;0.352799137153601]
Robot Arm 1 togoal: 11.7037708150659
Mobile Platform 2 Pose: [8.93827427480277;-3.54418514177694;2.81017548262423]
Robot Arm 2 end: [-2.91434307607283;3.17535365847877;0.487393844916914]
Robot Arm 1 togoal: 14.9954933147734

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.906604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-2.39225567948829;-2.00780873603251;8.16330639400522]
Robot Arm 1 end: [-1.57476985705022;8.74586890543773;0.20980319000706]
Robot Arm 1 togoal: 11.7161162131724
Mobile Platform 2 Pose: [9.06809523765339;-3.62279207821821;2.80815798503145]
Robot Arm 2 end: [-2.90913500406884;3.0783135738717;0.609261843916436]
Robot Arm 1 togoal: 14.9048287072444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-2.31256348422277;-1.99621449148542;8.27905994069736]
Robot Arm 1 end: [-1.45803016617762;8.83375468234231;0.33925574131965]
Robot Arm 1 togoal: 11.577698515807
Mobile Platform 2 Pose: [8.68227965139567;-3.3741722741824;2.83439141870539]
Robot Arm 2 end: [-2.71150297738968;3.18594149691274;0.419725513735507]
Robot Arm 1 togoal: 15.1064240474939

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-2.46766271999728;-1.8828913459947;8.34291204572002]
Robot Arm 1 end: [-1.44069484433117;8.91655370429358;0.206780629628507]
Robot Arm 1 togoal: 11.5667855063646
Mobile Platform 2 Pose: [8.34036455009454;-3.14102893220207;2.92463654266885]
Robot Arm 2 end: [-2.54800521579129;3.35800991097849;0.33637415135616]
Robot Arm 1 togoal: 15.3418380074536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-2.45225654617509;-1.82609376416722;8.47277102816587]
Robot Arm 1 end: [-1.31572635445717;8.99216140336982;0.229144273722613]
Robot Arm 1 togoal: 11.4337348571379
Mobile Platform 2 Pose: [8.01727985333102;-2.91972163029543;3.04092531307665]
Robot Arm 2 end: [-2.33891100781455;3.40551465294488;0.205041808071237]
Robot Arm 1 togoal: 15.4961883577675

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.086031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-2.63915444186019;-1.67083891756873;8.57337368144325]
Robot Arm 1 end: [-1.24446639157555;9.11780647908275;0.144920804978945]
Robot Arm 1 togoal: 11.3626530260582
Mobile Platform 2 Pose: [7.79586907150679;-2.72916340267808;3.20275102762198]
Robot Arm 2 end: [-2.11422294587979;3.45839565400762;0.206211664353172]
Robot Arm 1 togoal: 15.6538353669206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.678210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-2.55803823277683;-1.64775077731091;8.69078255383541]
Robot Arm 1 end: [-1.13528650039177;9.20879331480631;0.228227745814769]
Robot Arm 1 togoal: 11.2379657218597
Mobile Platform 2 Pose: [8.05598267809174;-2.87619814364822;3.22340765128696]
Robot Arm 2 end: [-2.23586952986911;3.46374081033466;0.296774703738767]
Robot Arm 1 togoal: 15.5901496510166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.432668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-2.75009668450379;-1.50497687552634;8.76721141880611]
Robot Arm 1 end: [-1.0753297242268;9.30725717602981;0.144961304272298]
Robot Arm 1 togoal: 11.1819633165488
Mobile Platform 2 Pose: [8.15080613156957;-2.9345025635103;3.22242752152696]
Robot Arm 2 end: [-2.24034366117491;3.47626346790668;0.45404930024344]
Robot Arm 1 togoal: 15.5871847152979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-2.76276737219229;-1.42101117129417;8.903565156464]
Robot Arm 1 end: [-0.919430158951173;9.40230524722517;0.179353601627653]
Robot Arm 1 togoal: 11.0177678806541
Mobile Platform 2 Pose: [8.64344263549043;-3.16467300467134;3.32000604383459]
Robot Arm 2 end: [-2.4464841078437;3.58552667766583;0.656972474475823]
Robot Arm 1 togoal: 15.568194532684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.557167e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-2.98779635448783;-1.24933661594229;8.98786535443783]
Robot Arm 1 end: [-0.81145740263729;9.52502745276633;0.145594422680327]
Robot Arm 1 togoal: 10.9089390222784
Mobile Platform 2 Pose: [8.35583812798085;-2.9825282005284;3.33484776053854]
Robot Arm 2 end: [-2.28422309587286;3.63845058757299;0.469895876057781]
Robot Arm 1 togoal: 15.7049472314714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-3.0061330204033;-1.15556413397224;9.11559160628065]
Robot Arm 1 end: [-0.648359263286555;9.61168220337892;0.178509233883997]
Robot Arm 1 togoal: 10.7396753753687
Mobile Platform 2 Pose: [7.9738163040262;-2.73535993492869;3.37236906757012]
Robot Arm 2 end: [-2.08872784619268;3.73337219117196;0.332643625677422]
Robot Arm 1 togoal: 15.8935813995857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.555396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-3.24476403903032;-0.974251555390648;9.19570630119187]
Robot Arm 1 end: [-0.523786407998652;9.73679339637048;0.164762541939397]
Robot Arm 1 togoal: 10.6140820667648
Mobile Platform 2 Pose: [7.65186694650126;-2.53094653123497;3.40963442496069]
Robot Arm 2 end: [-1.96078772160528;3.76712401199822;0.173034591883724]
Robot Arm 1 togoal: 15.9993639986191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-3.29378865218213;-0.860391643578465;9.31757894308811]
Robot Arm 1 end: [-0.348809343298651;9.80856984534335;0.177606973746658]
Robot Arm 1 togoal: 10.4373945576012
Mobile Platform 2 Pose: [7.35187012201239;-2.31160113970559;3.52958274389366]
Robot Arm 2 end: [-1.69951800893475;3.8698154463692;0.20342189171276]
Robot Arm 1 togoal: 16.2174533719625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.619665e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-3.54793876284914;-0.665499816561313;9.39623863640995]
Robot Arm 1 end: [-0.198468308832063;9.94631875637853;0.197340859808843]
Robot Arm 1 togoal: 10.2841485326487
Mobile Platform 2 Pose: [7.12374228889526;-2.17124332313915;3.54501876831033]
Robot Arm 2 end: [-1.56943769245618;3.84437687911604;0.14611450791771]
Robot Arm 1 togoal: 16.2674971064611

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-3.60389116804032;-0.549836294621754;9.50553980841858]
Robot Arm 1 end: [-0.0224423152636692;9.97650951737729;0.177090370715628]
Robot Arm 1 togoal: 10.1121706303466
Mobile Platform 2 Pose: [6.89290320185302;-2.02852303803639;3.55859695557236]
Robot Arm 2 end: [-1.38780312690532;3.82864714147075;0.166479235544752]
Robot Arm 1 togoal: 16.3473589073461

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.595443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-3.87383741267878;-0.345402648366299;9.57840282489695]
Robot Arm 1 end: [0.102961478885723;10.1458060503818;0.204629942299324]
Robot Arm 1 togoal: 9.98526219092712
Mobile Platform 2 Pose: [6.71333032551839;-1.91968326738949;3.56169433570482]
Robot Arm 2 end: [-1.26144132905022;3.78451598897139;0.162133249479455]
Robot Arm 1 togoal: 16.3774786686324

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-3.90550683583409;-0.250214885005172;9.67891315348555]
Robot Arm 1 end: [0.266284053553258;10.1621361466944;0.176898257143502]
Robot Arm 1 togoal: 9.82741694197037
Mobile Platform 2 Pose: [6.53303813391059;-1.80971899627659;3.56277357017327]
Robot Arm 2 end: [-1.14954344462458;3.78371841899728;0.161419002503242]
Robot Arm 1 togoal: 16.436846874676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-4.1660959875503;-0.053314144117409;9.75162943404136]
Robot Arm 1 end: [0.385549240229233;10.3294582470456;0.209056884398185]
Robot Arm 1 togoal: 9.7091400693974
Mobile Platform 2 Pose: [6.35987697013742;-1.70435188276609;3.56327240727236]
Robot Arm 2 end: [-1.04210234153523;3.78281127733702;0.161766604406478]
Robot Arm 1 togoal: 16.494160560329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-4.19975411325807;0.0386218728831753;9.85056302070165]
Robot Arm 1 end: [0.543891572184868;10.3455404167971;0.176734483053645]
Robot Arm 1 togoal: 9.55742823976658
Mobile Platform 2 Pose: [6.1925776000434;-1.60277381349207;3.56334261305419]
Robot Arm 2 end: [-0.938724732129747;3.78224412303219;0.162597089170751]
Robot Arm 1 togoal: 16.5499915714517

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.039697e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-4.45021812792459;0.228540844210547;9.92419317396609]
Robot Arm 1 end: [0.656768038239272;10.5111324971657;0.210670884742053]
Robot Arm 1 togoal: 9.44850264916186
Mobile Platform 2 Pose: [6.02945244235133;-1.50407264714069;3.56309980261583]
Robot Arm 2 end: [-0.889978597068826;3.77667529101532;0.109573016202202]
Robot Arm 1 togoal: 16.5765315481414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.557424e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-4.48078310582115;0.314263187547477;10.0210057425144]
Robot Arm 1 end: [0.80982643549137;10.5271413569806;0.17784774546332]
Robot Arm 1 togoal: 9.30275368559759
Mobile Platform 2 Pose: [5.8291132164207;-1.38021842175764;3.56830567135901]
Robot Arm 2 end: [-0.826523458802411;3.77495037046263;0.0666572768230436]
Robot Arm 1 togoal: 16.6137606313883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.617402e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-4.71882273279186;0.495841900312873;10.0957306036163]
Robot Arm 1 end: [0.949302265336291;10.6638151708956;0.210391250744139]
Robot Arm 1 togoal: 9.16915892320468
Mobile Platform 2 Pose: [5.73562523259641;-1.31228623425343;3.58994459035112]
Robot Arm 2 end: [-0.721805382398607;3.79645019350305;0.117007668473621]
Robot Arm 1 togoal: 16.6852667875249

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-4.79638455195857;0.610867258439718;10.1954505238221]
Robot Arm 1 end: [1.11190618315298;10.6997204586612;0.183721540614106]
Robot Arm 1 togoal: 9.01532773088592
Mobile Platform 2 Pose: [5.95761862590558;-1.41491320715419;3.65025066904831]
Robot Arm 2 end: [-0.795926039384285;3.8561642104308;0.175110384711919]
Robot Arm 1 togoal: 16.6889089803812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-5.04475800945506;0.799425121519283;10.2689062936689]
Robot Arm 1 end: [1.2606484120569;10.8382635126598;0.223291663253012]
Robot Arm 1 togoal: 8.87485204505421
Mobile Platform 2 Pose: [6.25943077557137;-1.58478525990161;3.6769623026046]
Robot Arm 2 end: [-0.907394348820173;3.89941298920332;0.326961955387704]
Robot Arm 1 togoal: 16.6521736065981

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-5.13399044490082;0.917572264116337;10.3624249209442]
Robot Arm 1 end: [1.42774005440375;10.8685324451098;0.197325795311126]
Robot Arm 1 togoal: 8.71723021281651
Mobile Platform 2 Pose: [6.70780281874586;-1.82875519954583;3.73153948246492]
Robot Arm 2 end: [-1.14732329073636;3.97436699034415;0.418520428372132]
Robot Arm 1 togoal: 16.5791519356515

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-5.37813729841591;1.10249709977164;10.4326676058561]
Robot Arm 1 end: [1.54498362677914;11.0282603484626;0.242551558784142]
Robot Arm 1 togoal: 8.6127261440557
Mobile Platform 2 Pose: [6.93667691215587;-1.96458681289882;3.7406296661103]
Robot Arm 2 end: [-1.24097019079163;3.98375118806377;0.611598215887717]
Robot Arm 1 togoal: 16.5255230505186

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-5.43857590099886;1.1959546145191;10.5187979563017]
Robot Arm 1 end: [1.7061239264815;11.0424238579769;0.218074169454979]
Robot Arm 1 togoal: 8.46006167486169
Mobile Platform 2 Pose: [6.61471998665333;-1.75712448254201;3.76060813479052]
Robot Arm 2 end: [-1.06535141053455;4.04858504867639;0.427088507134197]
Robot Arm 1 togoal: 16.6849432016221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-5.66015743772912;1.36484595944452;10.5882514817746]
Robot Arm 1 end: [1.80560739249329;11.1978312823623;0.262876038698113]
Robot Arm 1 togoal: 8.37649962030491
Mobile Platform 2 Pose: [6.29049330659452;-1.52534493878541;3.85430041353312]
Robot Arm 2 end: [-0.883573841290203;4.21563173828638;0.342427018330591]
Robot Arm 1 togoal: 16.9287461407779

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-5.71174856295751;1.44908186151923;10.6729835562949]
Robot Arm 1 end: [1.96195705905497;11.2162582583113;0.250020657415833]
Robot Arm 1 togoal: 8.22829307697911
Mobile Platform 2 Pose: [5.96921771528112;-1.31176910493728;3.92189095329998]
Robot Arm 2 end: [-0.715025559660189;4.24202960567331;0.191647089564272]
Robot Arm 1 togoal: 17.0532500364922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-5.90936863595015;1.60323714067206;10.7437438277333]
Robot Arm 1 end: [2.04393895304596;11.3685541252966;0.291056261981192]
Robot Arm 1 togoal: 8.1660644973829
Mobile Platform 2 Pose: [5.93080987870911;-1.21682422051493;4.10162819986726]
Robot Arm 2 end: [-0.579421159326541;4.30492645257837;0.211785912109791]
Robot Arm 1 togoal: 17.1782498536653

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-5.9454435793639;1.67440950580197;10.8260824106355]
Robot Arm 1 end: [2.19278606598204;11.3875328658661;0.289442694265799]
Robot Arm 1 togoal: 8.02462274893281
Mobile Platform 2 Pose: [6.23868085657336;-1.38970345972669;4.13044396903865]
Robot Arm 2 end: [-0.738803568152042;4.35015371453193;0.302084264824302]
Robot Arm 1 togoal: 17.1225706758485

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-6.11833939319863;1.81289965537804;10.8975759209089]
Robot Arm 1 end: [2.25536001200429;11.5355461382904;0.32197537731861]
Robot Arm 1 togoal: 7.98592577803457
Mobile Platform 2 Pose: [6.35322085445696;-1.45983251376815;4.13019903519545]
Robot Arm 2 end: [-0.756098163862169;4.36178571928142;0.458858053611277]
Robot Arm 1 togoal: 17.1125321246444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-6.13937827896123;1.87178097386752;10.9779042997239]
Robot Arm 1 end: [2.39579831799656;11.5552199080901;0.333269339016855]
Robot Arm 1 togoal: 7.85196130314861
Mobile Platform 2 Pose: [6.85619258557286;-1.67441343864435;4.25847618311892]
Robot Arm 2 end: [-0.950676370938311;4.50667090211673;0.635829628105094]
Robot Arm 1 togoal: 17.120668368761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-6.28926498306836;1.99480866321432;11.0485144918162]
Robot Arm 1 end: [2.4394717428646;11.6973456062366;0.354342972825944]
Robot Arm 1 togoal: 7.83604863773879
Mobile Platform 2 Pose: [6.58043836930117;-1.49792862887598;4.27690627290588]
Robot Arm 2 end: [-0.806400284827044;4.55772658677225;0.423483243528023]
Robot Arm 1 togoal: 17.2526584075841

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','1.690521e-14','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-6.30054792706724;2.04447341712794;11.1264466646229]
Robot Arm 1 end: [2.57215470399686;11.7169814087994;0.377455862634875]
Robot Arm 1 togoal: 7.70899501190789
Mobile Platform 2 Pose: [6.31436324515439;-1.28268604635166;4.40407006754989]
Robot Arm 2 end: [-0.59614523240124;4.82605965563972;0.595229636986772]
Robot Arm 1 togoal: 17.5812849407463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.327553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-6.43152935206269;2.15378240018251;11.1943924059796]
Robot Arm 1 end: [2.59981353748895;11.8520046926508;0.386514730909001]
Robot Arm 1 togoal: 7.71231080074686
Mobile Platform 2 Pose: [6.21906384635363;-1.2273265352794;4.40515508621621]
Robot Arm 2 end: [-0.502402210974435;4.74360164110833;0.522846413793029]
Robot Arm 1 togoal: 17.5662877038791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-6.43781564622021;2.19697011748041;11.2690486174157]
Robot Arm 1 end: [2.7267435642697;11.8686880151853;0.416846481577762]
Robot Arm 1 togoal: 7.59021798568614
Mobile Platform 2 Pose: [6.0870007268534;-1.14836147110848;4.40744952427963]
Robot Arm 2 end: [-0.412231002960978;4.68363116146031;0.443828924108464]
Robot Arm 1 togoal: 17.5697079074505

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968337e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-6.5532332724191;2.29488196403104;11.333658465241]
Robot Arm 1 end: [2.75162769330339;11.9895998753063;0.412800726527566]
Robot Arm 1 togoal: 7.59773087112395
Mobile Platform 2 Pose: [5.88051349998929;-1.02101170213782;4.41240586690773]
Robot Arm 2 end: [-0.291458784014311;4.68103981573698;0.391454907756028]
Robot Arm 1 togoal: 17.637022841998

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957772e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-6.56600751705036;2.3404848390512;11.4070713835624]
Robot Arm 1 end: [2.87367846847985;12.0074425324083;0.439726658893575]
Robot Arm 1 togoal: 7.48220797156275
Mobile Platform 2 Pose: [5.72383199496707;-0.927977653892705;4.41442963311137]
Robot Arm 2 end: [-0.197033002425913;4.64532897071636;0.343200816730791]
Robot Arm 1 togoal: 17.6627017619655

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-6.67473523679567;2.43445286524192;11.4707739730772]
Robot Arm 1 end: [2.89018523399299;12.128510027156;0.424228697043925]
Robot Arm 1 togoal: 7.5021948944041
Mobile Platform 2 Pose: [5.57769151404916;-0.839106060780523;4.41572043348414]
Robot Arm 2 end: [-0.140872846613646;4.62752680809528;0.257159220961836]
Robot Arm 1 togoal: 17.6851412847889

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999741e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-6.68792524137219;2.47769796270776;11.5404683800422]
Robot Arm 1 end: [3.01805747894144;12.1345387680464;0.448771181749443]
Robot Arm 1 togoal: 7.37925821193062
Mobile Platform 2 Pose: [5.35450440775348;-0.696347633511401;4.42188681783325]
Robot Arm 2 end: [-0.0523342114018411;4.6269888532468;0.160306537000104]
Robot Arm 1 togoal: 17.7413737984098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-6.79705755065807;2.5703341545309;11.6011248432101]
Robot Arm 1 end: [3.02292842863351;12.2583158950324;0.419030599321566]
Robot Arm 1 togoal: 7.4157841765897
Mobile Platform 2 Pose: [5.01739194737155;-0.469708020968838;4.43830354467072]
Robot Arm 2 end: [0.166123382650222;4.7109119996299;0.165991701423503]
Robot Arm 1 togoal: 17.9331269995317

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-6.80584858275813;2.60884727630823;11.6671402587953]
Robot Arm 1 end: [3.15679619189114;12.2507748851075;0.441127042387776]
Robot Arm 1 togoal: 7.28433604025299
Mobile Platform 2 Pose: [4.80927502980135;-0.337898618686913;4.44612293884387]
Robot Arm 2 end: [0.300800038405378;4.71095081392798;0.160079131918529]
Robot Arm 1 togoal: 18.0102931927884

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-6.92346031968261;2.70439421749342;11.725035972298]
Robot Arm 1 end: [3.14357914732434;12.3841688482734;0.396866034541534]
Robot Arm 1 togoal: 7.34564123670496
Mobile Platform 2 Pose: [4.6752767863984;-0.257304297634283;4.44800555668025]
Robot Arm 2 end: [0.407357593404086;4.64730972757948;0.160607830573267]
Robot Arm 1 togoal: 18.0196404289117

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-6.91796988193726;2.73094618690269;11.7844853211543]
Robot Arm 1 end: [3.2811345663126;12.3589072113076;0.420436498650811]
Robot Arm 1 togoal: 7.20547264015764
Mobile Platform 2 Pose: [4.5372520669172;-0.17349035233241;4.44798718343521]
Robot Arm 2 end: [0.49351650798071;4.64577136580617;0.161422690375227]
Robot Arm 1 togoal: 18.0682306290234

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-7.02917758059436;2.82078165402886;11.8390483624143]
Robot Arm 1 end: [3.24874558600616;12.4942622987245;0.364169434106956]
Robot Arm 1 togoal: 7.28965277194285
Mobile Platform 2 Pose: [4.40411509542056;-0.0928085060005475;4.44758413752643]
Robot Arm 2 end: [0.576096734719886;4.64411829695344;0.162198820983678]
Robot Arm 1 togoal: 18.1149210178078

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-7.01353707548082;2.83751901457013;11.8917473242008]
Robot Arm 1 end: [3.38615351385455;12.4558695008492;0.387763222064256]
Robot Arm 1 togoal: 7.14552209025807
Mobile Platform 2 Pose: [4.27482946863445;-0.0145947402305412;4.44690836380667]
Robot Arm 2 end: [0.655864609243604;4.64271428751229;0.163060210945625]
Robot Arm 1 togoal: 18.1604111392596

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-7.06179222379241;2.89402678005184;11.9446419376994]
Robot Arm 1 end: [3.32564062589775;12.5817491623369;0.324898932023677]
Robot Arm 1 togoal: 7.2555605956196
Mobile Platform 2 Pose: [4.14879922891488;0.0615075655131848;4.44603381331767]
Robot Arm 2 end: [0.733241661356055;4.64149387959394;0.164000070610594]
Robot Arm 1 togoal: 18.2048693594769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-7.04326528953104;2.90743731908022;11.994428987918]
Robot Arm 1 end: [3.45907680321716;12.5382333008243;0.346653010073678]
Robot Arm 1 togoal: 7.11374693170723
Mobile Platform 2 Pose: [4.02554191876045;0.135789530600848;4.44501849802431]
Robot Arm 2 end: [0.808561952075348;4.64039898974609;0.165002045899875]
Robot Arm 1 togoal: 18.2484252608912

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-7.08526418825842;2.96043605110767;12.047238849273]
Robot Arm 1 end: [3.40929347166044;12.6502897212042;0.279155473156792]
Robot Arm 1 togoal: 7.21135396287471
Mobile Platform 2 Pose: [3.90461338901615;0.208525313654117;4.44391355272618]
Robot Arm 2 end: [0.882129668934879;4.63938318767889;0.166040197325627]
Robot Arm 1 togoal: 18.2912056003444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-7.02285085273833;2.9527974300404;12.0981016100375]
Robot Arm 1 end: [3.51372837154354;12.6075708664547;0.299789580262909]
Robot Arm 1 togoal: 7.09665416594166
Mobile Platform 2 Pose: [3.78557984975356;0.279989151101572;4.44276653692662]
Robot Arm 2 end: [0.954241209390861;4.63841330021207;0.167080731561577]
Robot Arm 1 togoal: 18.3333478068462

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-7.06674857130811;3.00777533433674;12.1520258952203]
Robot Arm 1 end: [3.47220303208559;12.7118000177072;0.226569743106859]
Robot Arm 1 togoal: 7.18620499848912
Mobile Platform 2 Pose: [3.66801496985308;0.350455863324563;4.44162199335321]
Robot Arm 2 end: [1.02518962277238;4.63746906646078;0.168085841562243]
Robot Arm 1 togoal: 18.375001594824

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-7.04484022475605;3.02166509630706;12.2047282068646]
Robot Arm 1 end: [3.59618358001044;12.6654073607799;0.250909602765743]
Robot Arm 1 togoal: 7.05169417636461
Mobile Platform 2 Pose: [3.539139557418;0.426378109050869;4.44052074156798]
Robot Arm 2 end: [1.10148762956909;4.63639559283776;0.169018895358303]
Robot Arm 1 togoal: 18.4199608155616

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-7.09254546164535;3.08011021956654;12.2598034036649]
Robot Arm 1 end: [3.55683806246208;12.766309508994;0.169887793859895]
Robot Arm 1 togoal: 7.14088987777216
Mobile Platform 2 Pose: [3.39826640877987;0.51079008414962;4.44000836029313]
Robot Arm 2 end: [1.18552358954957;4.63596137682276;0.16929500782269]
Robot Arm 1 togoal: 18.4703657186883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-7.06123364810825;3.09027755342867;12.314187147742]
Robot Arm 1 end: [3.6763786966496;12.717740072261;0.203709108839473]
Robot Arm 1 togoal: 7.0078208296513
Mobile Platform 2 Pose: [3.24696723445064;0.601005572635464;4.43977103228636]
Robot Arm 2 end: [1.27532393390974;4.63552989002415;0.169478482666481]
Robot Arm 1 togoal: 18.5245317184593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-7.15181876200849;3.1711052948935;12.3695723025053]
Robot Arm 1 end: [3.68004709411837;12.8319063664621;0.154155610256446]
Robot Arm 1 togoal: 7.05901990270206
Mobile Platform 2 Pose: [3.1215182392413;0.675078234742443;4.43914061046711]
Robot Arm 2 end: [1.3497940602746;4.6344376342871;0.170268371692548]
Robot Arm 1 togoal: 18.5690351395594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-7.10286862627454;3.17048165455188;12.4172644821612]
Robot Arm 1 end: [3.74930136051384;12.8117370904808;0.179401487697622]
Robot Arm 1 togoal: 6.98405222745876
Mobile Platform 2 Pose: [2.98279557758091;0.758089471642235;4.43901704144275]
Robot Arm 2 end: [1.43253936586509;4.63389232797501;0.170424387933311]
Robot Arm 1 togoal: 18.6192861168932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.594668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-7.15932114384347;3.22953641217407;12.4675885618349]
Robot Arm 1 end: [3.7808912227323;12.8819375077059;0.159181571962314]
Robot Arm 1 togoal: 6.98837801015039
Mobile Platform 2 Pose: [2.8414211290104;0.843054879338229;4.43916106918297]
Robot Arm 2 end: [1.51719816370198;4.63346693594381;0.170459909914429]
Robot Arm 1 togoal: 18.6710515676614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.415200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-7.1373734299643;3.2444264605699;12.5142215929528]
Robot Arm 1 end: [3.8251242553261;12.893891805761;0.169060990683589]
Robot Arm 1 togoal: 6.9520816544776
Mobile Platform 2 Pose: [2.71137373124666;0.921747776830541;4.43937821422189]
Robot Arm 2 end: [1.58404325219806;4.63031091921931;0.153033502191885]
Robot Arm 1 togoal: 18.7111567788031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-7.17626094985393;3.29273356059375;12.562199803186]
Robot Arm 1 end: [3.87398084125245;12.9342768374738;0.164020939841593]
Robot Arm 1 togoal: 6.92672241843035
Mobile Platform 2 Pose: [2.54698163356264;1.02415024842277;4.44116549179632]
Robot Arm 2 end: [1.69757243980781;4.63343816093809;0.16931200907426]
Robot Arm 1 togoal: 18.7829117502948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-7.15582267657974;3.31049489892898;12.6092190329294]
Robot Arm 1 end: [3.90300485194684;12.9589702538224;0.163483606119038]
Robot Arm 1 togoal: 6.91170786116176
Mobile Platform 2 Pose: [2.43098452901897;1.09358241943458;4.44126674509538]
Robot Arm 2 end: [1.76802520347768;4.63242459482041;0.170336564297557]
Robot Arm 1 togoal: 18.8260075010432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-7.15834103484041;3.3415998872923;12.6581510193671]
Robot Arm 1 end: [3.94480234991505;12.9843003994996;0.16255401608124]
Robot Arm 1 togoal: 6.88598888666493
Mobile Platform 2 Pose: [2.31620891354182;1.16225308500195;4.44133826213531]
Robot Arm 2 end: [1.83743484412331;4.63151013755922;0.17109889270275]
Robot Arm 1 togoal: 18.868709166073

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-7.16383724111355;3.37586224482775;12.7085844837429]
Robot Arm 1 end: [3.9881745820848;13.010549417145;0.160462941708047]
Robot Arm 1 togoal: 6.85977504992323
Mobile Platform 2 Pose: [2.20233841673836;1.23036563218391;4.44141773056131]
Robot Arm 2 end: [1.90610959048371;4.63067559418906;0.171686520725194]
Robot Arm 1 togoal: 18.9111808406657

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-7.17128171776155;3.41261486631389;12.7603423903849]
Robot Arm 1 end: [4.03512284694153;13.0372708910948;0.160650101971478]
Robot Arm 1 togoal: 6.83047039097871
Mobile Platform 2 Pose: [2.08918076862357;1.29804411206366;4.44152734182524]
Robot Arm 2 end: [1.97423846939778;4.62990900953692;0.17215490127531]
Robot Arm 1 togoal: 18.9535223019282

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-7.20993864162489;3.46593365947987;12.8120554300889]
Robot Arm 1 end: [4.09361869672407;13.0698527754443;0.157875300928338]
Robot Arm 1 togoal: 6.79466298706635
Mobile Platform 2 Pose: [1.97662009852771;1.36536374028279;4.44167994825866]
Robot Arm 2 end: [2.04193866617967;4.62920258752836;0.17254127651014]
Robot Arm 1 togoal: 18.9957941691295

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-7.24971848483137;3.51961861460928;12.8627077892546]
Robot Arm 1 end: [4.15329449918876;13.103134640745;0.157669385237801]
Robot Arm 1 togoal: 6.75811217552326
Mobile Platform 2 Pose: [1.86458949815721;1.43236836566377;4.44188250323131]
Robot Arm 2 end: [2.10928160982065;4.62855107898911;0.17287157164958]
Robot Arm 1 togoal: 19.0380318845163

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-7.29484057082465;3.57481601196997;12.9104950583923]
Robot Arm 1 end: [4.20987743120005;13.1419537748617;0.154953565839772]
Robot Arm 1 togoal: 6.72781359323937
Mobile Platform 2 Pose: [1.75305351402413;1.49908160520264;4.44213820460454]
Robot Arm 2 end: [2.17630900113661;4.62795089694575;0.173164154927197]
Robot Arm 1 togoal: 19.0802544479857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-7.3401620689549;3.62789239450485;12.9546532188193]
Robot Arm 1 end: [4.26493163459844;13.1809981340446;0.154781856082921]
Robot Arm 1 togoal: 6.69897830280059
Mobile Platform 2 Pose: [1.64199630887038;1.5655143906766;4.44244792526687]
Robot Arm 2 end: [2.24304340076579;4.62739960830867;0.173432074112202]
Robot Arm 1 togoal: 19.1224702988151

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-7.38678144544814;3.67847572925384;12.9937350062355]
Robot Arm 1 end: [4.31499574989181;13.2212122515213;0.153936866217487]
Robot Arm 1 togoal: 6.67565302459964
Mobile Platform 2 Pose: [1.53141348133476;1.63167020076248;4.44281119492156]
Robot Arm 2 end: [2.30949549412513;4.62689562663348;0.173684523183828]
Robot Arm 1 togoal: 19.1646814214686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-7.43028651236986;3.72434085362947;13.0282732713558]
Robot Arm 1 end: [4.36131729916594;13.2574712962928;0.155174347819427]
Robot Arm 1 togoal: 6.65367128426188
Mobile Platform 2 Pose: [1.42130648423259;1.69754864875147;4.44322687164363]
Robot Arm 2 end: [2.37566909899223;4.62643801314148;0.17392787476244]
Robot Arm 1 togoal: 19.2068862106957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.413301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-7.47030176632585;3.76585649884505;13.0590808343565]
Robot Arm 1 end: [4.40366955130665;13.2884668220373;0.15694288055438]
Robot Arm 1 togoal: 6.63276566416474
Mobile Platform 2 Pose: [1.31167698656607;1.76314883791134;4.44369358916199]
Robot Arm 2 end: [2.44428642736337;4.62351885818385;0.17560708433704]
Robot Arm 1 togoal: 19.2488350447354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.875392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-7.4736096030663;3.78770710804694;13.0881637356037]
Robot Arm 1 end: [4.42764075468209;13.312954152233;0.159158843826097]
Robot Arm 1 togoal: 6.62430857586865
Mobile Platform 2 Pose: [1.19734373705538;1.83196337731444;4.44375087547593]
Robot Arm 2 end: [2.51690620042276;4.61998629626023;0.178285336243951]
Robot Arm 1 togoal: 19.2929993210305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.911326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-7.47393234629277;3.80832499886287;13.1178022062708]
Robot Arm 1 end: [4.45215898375054;13.3305820434179;0.161538740718067]
Robot Arm 1 togoal: 6.61206863281121
Mobile Platform 2 Pose: [1.08482885505679;1.89976865134926;4.44335411310726]
Robot Arm 2 end: [2.58931667077526;4.61624486605493;0.182179230470617]
Robot Arm 1 togoal: 19.3369581184441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.930277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-7.4497245071826;3.81839820224314;13.1500524445406]
Robot Arm 1 end: [4.46912164352702;13.3413696162256;0.165718930432003]
Robot Arm 1 togoal: 6.6024353903591
Mobile Platform 2 Pose: [0.976818644111879;1.96477252862969;4.44248326512073]
Robot Arm 2 end: [2.6594934512281;4.61257891416758;0.187293971689745]
Robot Arm 1 togoal: 19.3796029044564

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-7.4254311038648;3.83151334969744;13.1865037130294]
Robot Arm 1 end: [4.49335334885099;13.3471084273189;0.174989009085998]
Robot Arm 1 togoal: 6.58316320263299
Mobile Platform 2 Pose: [0.873748065841671;2.02665858158632;4.44118004236952]
Robot Arm 2 end: [2.72690979559975;4.60913300146675;0.193619597592875]
Robot Arm 1 togoal: 19.4206832715168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951141e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-7.40805005389125;3.85189824540301;13.227073967223]
Robot Arm 1 end: [4.52085844646997;13.355499956595;0.18024986583213]
Robot Arm 1 togoal: 6.5633819822253
Mobile Platform 2 Pose: [0.765895249597871;2.09028714142844;4.4395082405121]
Robot Arm 2 end: [2.79647735571808;4.60570166749861;0.201374654942103]
Robot Arm 1 togoal: 19.4632413787297

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-7.39266071398308;3.87788681994836;13.2731487262078]
Robot Arm 1 end: [4.55399532772397;13.3612134130189;0.187067106519448]
Robot Arm 1 togoal: 6.53728532822454
Mobile Platform 2 Pose: [0.671935019544781;2.14632550601731;4.43752726546372]
Robot Arm 2 end: [2.84734944110544;4.60724641139321;0.198574044628612]
Robot Arm 1 togoal: 19.4980728738117

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963531e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-7.38263032271066;3.91234942334221;13.3254336063107]
Robot Arm 1 end: [4.59128520998294;13.3677350500336;0.190509598036988]
Robot Arm 1 togoal: 6.50892001461762
Mobile Platform 2 Pose: [0.582906815828616;2.20366477496089;4.43649785711205]
Robot Arm 2 end: [2.89701116569394;4.61056415700864;0.192867111534481]
Robot Arm 1 togoal: 19.5336995718559

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-7.37223006032805;3.95415809708131;13.3862342348645]
Robot Arm 1 end: [4.63458322891516;13.3696214255581;0.198160907303275]
Robot Arm 1 togoal: 6.4723990099642
Mobile Platform 2 Pose: [0.485622699337861;2.261472477207;4.43638073451719]
Robot Arm 2 end: [2.93311662059866;4.58280004592318;0.153445546262967]
Robot Arm 1 togoal: 19.5395436969577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.878682e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-7.40651202079704;4.02852501630983;13.4566576613137]
Robot Arm 1 end: [4.62884857748259;13.2729732235951;0.140355238043954]
Robot Arm 1 togoal: 6.43954977344676
Mobile Platform 2 Pose: [0.35163607261058;2.34344975525466;4.43806865513398]
Robot Arm 2 end: [2.99995538337491;4.583110150376;0.132887121808962]
Robot Arm 1 togoal: 19.5855252416252

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.032530e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-7.18209039133102;3.9364295893274;13.5013197559842]
Robot Arm 1 end: [4.58796303559871;13.2184152384185;0.307538460559322]
Robot Arm 1 togoal: 6.41255250728071
Mobile Platform 2 Pose: [0.199659265179685;2.43761154635578;4.44149826816195]
Robot Arm 2 end: [3.12057476073629;4.61710914913037;0.178080695304844]
Robot Arm 1 togoal: 19.6878844518293

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-7.37964345022757;4.28091949754582;13.6800124887466]
Robot Arm 1 end: [4.77802327827836;13.2197198583206;0.194550041022495]
Robot Arm 1 togoal: 6.27655210620753
Mobile Platform 2 Pose: [0.097795211074033;2.49852341109526;4.44239664836882]
Robot Arm 2 end: [3.18201872098007;4.61743946387153;0.179532961406691]
Robot Arm 1 togoal: 19.7290315995451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.018478e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-8.19057303088773;4.77078455614971;13.6208021356286]
Robot Arm 1 end: [5.16905083469991;13.1241764020669;0.0951946955704425]
Robot Arm 1 togoal: 5.92718050974363
Mobile Platform 2 Pose: [-0.00553985190264826;2.5604362216298;4.44359255889169]
Robot Arm 2 end: [3.24414305753847;4.61813398002067;0.180377531709715]
Robot Arm 1 togoal: 19.7710504270477

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.635728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-8.93405519528214;5.23613690128371;13.5488092781531]
Robot Arm 1 end: [5.58713352519743;13.1184548818346;-0.00609065879862386]
Robot Arm 1 togoal: 5.61517205922172
Mobile Platform 2 Pose: [-0.110201157628163;2.62326253713076;4.44508136539508]
Robot Arm 2 end: [3.3070551740021;4.61912150666358;0.180872644749579]
Robot Arm 1 togoal: 19.8139438870413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.842638e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-9.35747777906433;5.51420357436224;13.5023561042443]
Robot Arm 1 end: [5.80329031914244;13.0449817361975;-0.0389336696676617]
Robot Arm 1 togoal: 5.41458022130456
Mobile Platform 2 Pose: [-0.215908908127492;2.68682465830388;4.44683751135503]
Robot Arm 2 end: [3.37069501741991;4.62034296443918;0.181203044826056]
Robot Arm 1 togoal: 19.8576174182045

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.658732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958073e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-9.34526988853939;5.516212134784;13.477773384956]
Robot Arm 1 end: [5.65485542940925;13.0239256240251;-0.0850846328726448]
Robot Arm 1 togoal: 5.53208056403011
Mobile Platform 2 Pose: [-0.34305708668754;2.76127413983944;4.44882682388102]
Robot Arm 2 end: [3.44149480792807;4.62085451533972;0.175566252945689]
Robot Arm 1 togoal: 19.9061136688707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.382533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-9.09617886035261;5.4705993281598;13.3438439276871]
Robot Arm 1 end: [5.50064424580073;12.8424015749321;-0.0513845762972817]
Robot Arm 1 togoal: 5.54940016324288
Mobile Platform 2 Pose: [-0.478753908579713;2.84159071912636;4.45102871793905]
Robot Arm 2 end: [3.52553007920306;4.62276291148557;0.181374934512112]
Robot Arm 1 togoal: 19.9639613905915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.905008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986750e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-8.91483729083605;5.35247484671569;13.3100957886441]
Robot Arm 1 end: [5.32448094633982;12.6583568993147;0.138944509331267]
Robot Arm 1 togoal: 5.55314480313284
Mobile Platform 2 Pose: [-0.609729031155938;2.91933012653555;4.4529519642867]
Robot Arm 2 end: [3.60305676648739;4.6238450180639;0.181626929029945]
Robot Arm 1 togoal: 20.0173404009591

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.647053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-8.88701364955509;5.1179757083757;13.1680058478786]
Robot Arm 1 end: [4.88268768789656;12.7093325964059;0.326360618897696]
Robot Arm 1 togoal: 5.91223575886766
Mobile Platform 2 Pose: [-0.737951459985365;2.99524692072555;4.45563345930635]
Robot Arm 2 end: [3.67934457598557;4.6254661356806;0.181754875049767]
Robot Arm 1 togoal: 20.0704336081508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.548175e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-8.68997796016579;5.0089270331971;13.1221004902354]
Robot Arm 1 end: [4.57405755113142;12.7414746853821;0.279706224717728]
Robot Arm 1 togoal: 6.20462288504138
Mobile Platform 2 Pose: [-0.862048451272643;3.06878022621421;4.45854275785276]
Robot Arm 2 end: [3.75350840935355;4.6273049212127;0.182116901907374]
Robot Arm 1 togoal: 20.1223669277411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.028587e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-8.46264607159344;4.9271945372449;13.064196071012]
Robot Arm 1 end: [4.29646794598112;12.7398140806785;0.380129680458379]
Robot Arm 1 togoal: 6.42949797215272
Mobile Platform 2 Pose: [-0.980733732815349;3.1394485983483;4.46140983648019]
Robot Arm 2 end: [3.81530558869895;4.62770912422322;0.168215322112329]
Robot Arm 1 togoal: 20.1658763537999

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.265517e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-8.1591859368881;4.92041133786949;12.9263161879054]
Robot Arm 1 end: [4.2274958694445;12.6501923828292;0.440090125174281]
Robot Arm 1 togoal: 6.44311180725446
Mobile Platform 2 Pose: [-1.11312537555908;3.21918231431795;4.46490076489968]
Robot Arm 2 end: [3.90498495954924;4.63145457182497;0.182498674725998]
Robot Arm 1 togoal: 20.2291783177278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.213666e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-7.65377710402941;4.63869305941457;12.7243854471697]
Robot Arm 1 end: [3.87115131507587;12.5414191383774;0.490874244779262]
Robot Arm 1 togoal: 6.71436940532589
Mobile Platform 2 Pose: [-1.23262981497165;3.29076746545456;4.46697875621844]
Robot Arm 2 end: [3.9765816704209;4.63276765725993;0.183381935979123]
Robot Arm 1 togoal: 20.2793477850372

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.513247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-6.91750915597973;4.00073668449906;12.6363904910149]
Robot Arm 1 end: [3.27030129274125;12.5220798160491;0.386732126407155]
Robot Arm 1 togoal: 7.27573328937139
Mobile Platform 2 Pose: [-1.3519334237293;3.36270937495054;4.46945341582271]
Robot Arm 2 end: [4.04838526198566;4.63448822718019;0.18375323683315]
Robot Arm 1 togoal: 20.3301156673112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-6.65194793491784;3.75448975980076;12.6248760415151]
Robot Arm 1 end: [2.9688151772211;12.3616310482266;0.458636308014595]
Robot Arm 1 togoal: 7.49289515701863
Mobile Platform 2 Pose: [-1.47021679424779;3.43448091875689;4.47228021214923]
Robot Arm 2 end: [4.12006805837872;4.63649586352521;0.183907203995586]
Robot Arm 1 togoal: 20.3811469324316

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.507949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-7.06537410476449;3.6618147974701;12.5855600758456]
Robot Arm 1 end: [3.02127310246235;12.0708343632567;0.383491646780786]
Robot Arm 1 togoal: 7.3678293495509
Mobile Platform 2 Pose: [-1.58986907651485;3.50755556421445;4.47497182788188]
Robot Arm 2 end: [4.14768342295232;4.63170700555188;0.120564173824912]
Robot Arm 1 togoal: 20.4011033190764

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.640836e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.288725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [-7.80010587363698;3.90552085251894;12.5298163872817]
Robot Arm 1 end: [3.35551755829283;11.9459649598025;0.314915306779746]
Robot Arm 1 togoal: 7.02784225990839
Mobile Platform 2 Pose: [-1.77342865973197;3.62468525839859;4.48362446596907]
Robot Arm 2 end: [4.26278070213072;4.63950614799429;0.114559856351254]
Robot Arm 1 togoal: 20.4870722039998

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.816293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [-8.69789395105034;4.19799800557175;12.4005579104093]
Robot Arm 1 end: [3.9119617997934;11.6453786331626;0.302702301451783]
Robot Arm 1 togoal: 6.42306229581581
Mobile Platform 2 Pose: [-1.94200376744167;3.73146919079118;4.49318318792727]
Robot Arm 2 end: [4.42226647563222;4.64936910902784;0.181444093603054]
Robot Arm 1 togoal: 20.6009755985741

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.748502e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [-9.178642376595;4.35802975686098;12.0044801030043]
Robot Arm 1 end: [4.21193680653677;11.2608524063293;0.223201602295461]
Robot Arm 1 togoal: 6.06429756881658
Mobile Platform 2 Pose: [-2.0257512299489;3.78135417527397;4.4938907957932]
Robot Arm 2 end: [4.47322705448712;4.64970667741936;0.185284680046619]
Robot Arm 1 togoal: 20.6366746963052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.043950e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [-8.58199964546492;4.1413478123518;11.8293643950761]
Robot Arm 1 end: [3.61456373292829;11.257086959398;0.30375594261782]
Robot Arm 1 togoal: 6.62085697168843
Mobile Platform 2 Pose: [-2.11460032499891;3.83463279191926;4.49536059808124]
Robot Arm 2 end: [4.52667458117568;4.65114789193502;0.187444934290954]
Robot Arm 1 togoal: 20.6750762526286
Step 324:
Mobile Platform 1 Pose: [-8.55624689578965;4.08332428071667;11.5861910622933]
Robot Arm 1 end: [3.49021686314146;11.1676619277388;0.237189749019335]
Robot Arm 1 togoal: 6.73712695642153
Mobile Platform 2 Pose: [-2.20839955675523;3.89125227405189;4.49765604492521]
Robot Arm 2 end: [4.58301912360474;4.65357970580065;0.188489020950132]
Robot Arm 1 togoal: 20.7163565923876
Step 325:
Mobile Platform 1 Pose: [-8.10306033091391;3.93478159294892;11.3851063290495]
Robot Arm 1 end: [3.26684447803158;11.0332616665488;0.363267015646918]
Robot Arm 1 togoal: 6.90965691026928
Mobile Platform 2 Pose: [-2.30652874391874;3.95084563863162;4.50077306686839]
Robot Arm 2 end: [4.64227221214279;4.65682764478953;0.188970299751886]
Robot Arm 1 togoal: 20.7603717516305
Step 326:
Mobile Platform 1 Pose: [-7.71591619454235;3.78811790406635;11.1826469936268]
Robot Arm 1 end: [3.0970446763793;11.02117365962;0.311333171018387]
Robot Arm 1 togoal: 7.08215233387023
Mobile Platform 2 Pose: [-2.40803950687624;4.01281384575563;4.50463660412004]
Robot Arm 2 end: [4.70407386824029;4.66070543092037;0.189322790257167]
Robot Arm 1 togoal: 20.8067161331584
Step 327:
Mobile Platform 1 Pose: [-7.20567169428344;3.58740368871119;11.0336040342002]
Robot Arm 1 end: [2.87187828966432;10.930607606425;0.286548507081774]
Robot Arm 1 togoal: 7.29383439096769
Mobile Platform 2 Pose: [-2.51193578620666;4.07650971621281;4.50913304265013]
Robot Arm 2 end: [4.76785841833387;4.66507218123475;0.189794242989171]
Robot Arm 1 togoal: 20.8548817573315
Step 328:
Mobile Platform 1 Pose: [-6.95542381862982;3.33775552539556;11.0203436907296]
Robot Arm 1 end: [2.61227875679572;10.8749237752521;0.319456311577737]
Robot Arm 1 togoal: 7.53575636648243
Mobile Platform 2 Pose: [-2.61744863578969;4.14142811322946;4.51416215375659]
Robot Arm 2 end: [4.83309199688056;4.66986196401584;0.190468434958353]
Robot Arm 1 togoal: 20.9044440843753
Step 329:
Mobile Platform 1 Pose: [-6.91742267343627;3.21333814009473;11.0867365875156]
Robot Arm 1 end: [2.55301847957967;10.7878780026212;0.30467461432869]
Robot Arm 1 togoal: 7.58668128738283
Mobile Platform 2 Pose: [-2.72415485053357;4.20729500202392;4.51967317035163]
Robot Arm 2 end: [4.89943423382235;4.67507741241139;0.191340263938095]
Robot Arm 1 togoal: 20.9551672043429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.937879e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [-6.85920897795673;3.01183744206407;11.1930901495658]
Robot Arm 1 end: [2.39882695159848;10.8730178011719;0.297562365876884]
Robot Arm 1 togoal: 7.74834120554183
Mobile Platform 2 Pose: [-2.83193457436185;4.27404560552956;4.52567092799285]
Robot Arm 2 end: [4.93291582996601;4.6660128576956;0.139529145627902]
Robot Arm 1 togoal: 20.9759950907799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-6.50301260758567;2.81774370820433;11.076960747363]
Robot Arm 1 end: [2.26658754767332;10.7211543714148;0.277592053478621]
Robot Arm 1 togoal: 7.86586264224621
Mobile Platform 2 Pose: [-2.99087394402923;4.37650109408403;4.53921818119895]
Robot Arm 2 end: [5.01749890261472;4.68242128879063;0.114538406881106]
Robot Arm 1 togoal: 21.0493919723227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-6.24865512723494;2.59267929421997;10.9681213723286]
Robot Arm 1 end: [2.11097925215658;10.59237306998;0.228062978404559]
Robot Arm 1 togoal: 8.01618614777336
Mobile Platform 2 Pose: [-3.16204707347372;4.4877908677204;4.5552407871429]
Robot Arm 2 end: [5.12397814720628;4.69997766415201;0.105637424220208]
Robot Arm 1 togoal: 21.1382901282166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-5.91231533974018;2.35465682744646;10.8916667757976]
Robot Arm 1 end: [1.94578403151918;10.4584514252592;0.232365970007572]
Robot Arm 1 togoal: 8.16952570468495
Mobile Platform 2 Pose: [-3.32037369871659;4.58944728169537;4.56791053189582]
Robot Arm 2 end: [5.22954823814685;4.70813751401879;0.107348324186696]
Robot Arm 1 togoal: 21.2194924475082
Step 334:
Mobile Platform 1 Pose: [-5.54585076106625;2.15179011659451;10.8286394931118]
Robot Arm 1 end: [1.85358750655074;10.3204711676285;0.273860412302738]
Robot Arm 1 togoal: 8.24755087489062
Mobile Platform 2 Pose: [-3.46411340886676;4.68062606335389;4.57728397578316]
Robot Arm 2 end: [5.38428633362056;4.72441737118781;0.197087693384178]
Robot Arm 1 togoal: 21.3362948215329
Step 335:
Mobile Platform 1 Pose: [-5.28752306581467;2.02157305967021;10.7698611887488]
Robot Arm 1 end: [1.87392543540902;10.2112535777103;0.312946739628329]
Robot Arm 1 togoal: 8.21809640873618
Mobile Platform 2 Pose: [-3.54008608023199;4.72635052497169;4.57924234119481]
Robot Arm 2 end: [5.43028289829601;4.72677172531514;0.203115461605574]
Robot Arm 1 togoal: 21.3707336193105
Step 336:
Mobile Platform 1 Pose: [-5.20696608256952;1.98559052450942;10.722503504555]
Robot Arm 1 end: [1.99816177503378;10.1568008569909;0.375382084350294]
Robot Arm 1 togoal: 8.08495157041409
Mobile Platform 2 Pose: [-3.63123139791763;4.78256569347179;4.58395144540866]
Robot Arm 2 end: [5.48523368822049;4.73317680688254;0.205830802248319]
Robot Arm 1 togoal: 21.4146826084436
Step 337:
Mobile Platform 1 Pose: [-5.26062609800327;2.01359760927878;10.694888698968]
Robot Arm 1 end: [2.17217950148253;10.1655876967065;0.452952909183979]
Robot Arm 1 togoal: 7.90208311969138
Mobile Platform 2 Pose: [-3.73820078928801;4.85036783496247;4.59260797999154]
Robot Arm 2 end: [5.55161115699694;4.74391332681128;0.206964279568402]
Robot Arm 1 togoal: 21.4700321294945
Step 338:
Mobile Platform 1 Pose: [-5.34516337729431;2.05937928292126;10.6812964300914]
Robot Arm 1 end: [2.34675909383338;10.2281124255022;0.527125564978907]
Robot Arm 1 togoal: 7.72087547102522
Mobile Platform 2 Pose: [-3.8586501682063;4.9289697787059;4.60618121304311]
Robot Arm 2 end: [5.62795877129907;4.76819010596822;0.208813624625998]
Robot Arm 1 togoal: 21.5419211642592
Step 339:
Mobile Platform 1 Pose: [-5.43840550907981;2.11589389788324;10.6746915092764]
Robot Arm 1 end: [2.54372633538396;10.2981678869992;0.653667560526197]
Robot Arm 1 togoal: 7.51246874276678
Mobile Platform 2 Pose: [-3.98993225987844;5.01797601915375;4.62712726169308]
Robot Arm 2 end: [5.71173237613498;4.81667013798203;0.212368339834849]
Robot Arm 1 togoal: 21.6357289005719
Step 340:
Mobile Platform 1 Pose: [-5.551795402164;2.19094584120415;10.6747040483625]
Robot Arm 1 end: [2.72293474824914;10.4236645059676;0.836442745779773]
Robot Arm 1 togoal: 7.32146084645422
Mobile Platform 2 Pose: [-4.12436800069597;5.11687097186282;4.66398199019871]
Robot Arm 2 end: [5.80299285406808;4.88896002040462;0.218308008404885]
Robot Arm 1 togoal: 21.7511544534974
Step 341:
Mobile Platform 1 Pose: [-5.68120483834434;2.27867562982268;10.6849449030999]
Robot Arm 1 end: [2.86420280127296;10.550972922331;0.942487129613506]
Robot Arm 1 togoal: 7.18037951390492
Mobile Platform 2 Pose: [-4.21138944789689;5.20241377503685;4.73020731324637]
Robot Arm 2 end: [5.89483654585622;4.93311874954747;0.227298906755393]
Robot Arm 1 togoal: 21.8475977381092
Step 342:
Mobile Platform 1 Pose: [-5.802925312365;2.35933995855506;10.7016174183675]
Robot Arm 1 end: [2.9956861381582;10.6869505397414;0.997857254079535]
Robot Arm 1 togoal: 7.05733604498534
Mobile Platform 2 Pose: [-4.25532542154946;5.27413786364456;4.81472612944229]
Robot Arm 2 end: [5.96720761630714;4.96887170825536;0.226263334351265]
Robot Arm 1 togoal: 21.9247618530701
Step 343:
Mobile Platform 1 Pose: [-5.93189363528912;2.43986552795995;10.7156573721427]
Robot Arm 1 end: [3.08597740977225;10.8047484694653;1.06009022611286]
Robot Arm 1 togoal: 6.9759419648497
Mobile Platform 2 Pose: [-4.14323409686564;5.26602079996269;4.91600377189804]
Robot Arm 2 end: [5.96308492404693;5.03811953020016;0.264986836772638]
Robot Arm 1 togoal: 21.9668543119656
Step 344:
Mobile Platform 1 Pose: [-6.09563681908616;2.53892067009398;10.728941379872]
Robot Arm 1 end: [3.11939471255407;10.9065799996065;1.18266610625643]
Robot Arm 1 togoal: 6.94831535272912
Mobile Platform 2 Pose: [-3.91206494198661;5.17757401800843;5.01025827418355]
Robot Arm 2 end: [5.87024389211052;5.17339097397094;0.315950529012609]
Robot Arm 1 togoal: 21.9897380400854
Step 345:
Mobile Platform 1 Pose: [-6.26410049720415;2.63707127096061;10.7390275920749]
Robot Arm 1 end: [3.10973805478611;10.9674779016747;1.30016336619348]
Robot Arm 1 togoal: 6.96135705036903
Mobile Platform 2 Pose: [-3.68919879521954;5.05417307304778;5.03483864422853]
Robot Arm 2 end: [5.75176445108748;5.16499470691152;0.356235679279717]
Robot Arm 1 togoal: 21.8963883712822
Step 346:
Mobile Platform 1 Pose: [-6.40541192828772;2.71490429655431;10.7413149596403]
Robot Arm 1 end: [3.153132493039;11.0316851390213;1.31776778597297]
Robot Arm 1 togoal: 6.92714026253071
Mobile Platform 2 Pose: [-3.40660481987237;4.89002609051099;5.06069633096078]
Robot Arm 2 end: [5.60100408358019;5.2103774316709;0.407249643454632]
Robot Arm 1 togoal: 21.8171343188439
Step 347:
Mobile Platform 1 Pose: [-6.5377346701899;2.78578463075996;10.7394083941281]
Robot Arm 1 end: [3.23488725245266;11.1111718366698;1.27853289926877]
Robot Arm 1 togoal: 6.86004691183469
Mobile Platform 2 Pose: [-3.11364731870475;4.72260357370063;5.0921979197997]
Robot Arm 2 end: [5.45269078847976;5.23839142471623;0.469337817336756]
Robot Arm 1 togoal: 21.7278673404381
Step 348:
Mobile Platform 1 Pose: [-6.6845344451716;2.86647821398821;10.741092806173]
Robot Arm 1 end: [3.33100337312049;11.1387680298939;1.24500826298327]
Robot Arm 1 togoal: 6.77115057240202
Mobile Platform 2 Pose: [-2.7511451191717;4.53691386175035;5.18174909417098]
Robot Arm 2 end: [5.26266018928175;5.37174327614861;0.52807988520098]
Robot Arm 1 togoal: 21.6846761044678
Step 349:
Mobile Platform 1 Pose: [-6.85904531152101;2.96501779962918;10.7477193911281]
Robot Arm 1 end: [3.43834452705808;11.1043776098023;1.25036344025232]
Robot Arm 1 togoal: 6.65944566747815
Mobile Platform 2 Pose: [-2.44521044484083;4.37166680973146;5.24426945733259]
Robot Arm 2 end: [5.10939170027822;5.39872618301704;0.516896552359541]
Robot Arm 1 togoal: 21.5968217417641
Step 350:
Mobile Platform 1 Pose: [-7.11233230054458;3.11525654631511;10.7724433847343]
Robot Arm 1 end: [3.53421825660367;11.0151159126435;1.33840253294307]
Robot Arm 1 togoal: 6.54752897697406
Mobile Platform 2 Pose: [-2.16431033052096;4.24703154536949;5.36855132867619]
Robot Arm 2 end: [4.96476690694935;5.56779251881258;0.495034245832204]
Robot Arm 1 togoal: 21.618349109422
Step 351:
Mobile Platform 1 Pose: [-6.86526484445044;3.01411802939704;10.9792332053451]
Robot Arm 1 end: [3.56658223318108;11.1536898854106;1.23385779037932]
Robot Arm 1 togoal: 6.54234783259541
Mobile Platform 2 Pose: [-1.88385482455541;4.09197009692164;5.4188206495605]
Robot Arm 2 end: [4.82190483408414;5.57539934284451;0.479677151260589]
Robot Arm 1 togoal: 21.525944366625
Step 352:
Mobile Platform 1 Pose: [-6.35731626466935;2.82810517963029;11.1462638005115]
Robot Arm 1 end: [3.49004856456104;11.2779347055432;1.08441925764428]
Robot Arm 1 togoal: 6.64854777737224
Mobile Platform 2 Pose: [-1.5925986261728;3.94792591659389;5.50933802741246]
Robot Arm 2 end: [4.66271662983507;5.70074863134637;0.462542140010103]
Robot Arm 1 togoal: 21.5088144627486
Step 353:
Mobile Platform 1 Pose: [-6.07287727262697;2.70052919575406;11.2180023661148]
Robot Arm 1 end: [3.40926805228015;11.182100000415;0.979041725006689]
Robot Arm 1 togoal: 6.71779925195074
Mobile Platform 2 Pose: [-1.31319400885425;3.79822958815874;5.57240943321834]
Robot Arm 2 end: [4.52101667792513;5.71375049884273;0.43837737076374]
Robot Arm 1 togoal: 21.4232105870664
Step 354:
Mobile Platform 1 Pose: [-5.73659078915691;2.64271593516966;11.4612257755574]
Robot Arm 1 end: [3.27279118780254;11.3466396665386;1.05998482463221]
Robot Arm 1 togoal: 6.87614076254418
Mobile Platform 2 Pose: [-1.06932492014973;3.6900470024394;5.67150374424356]
Robot Arm 2 end: [4.38882320747355;5.86499326249566;0.408732178293104]
Robot Arm 1 togoal: 21.4469900970315
Step 355:
Mobile Platform 1 Pose: [-5.51526992319594;2.45725572099707;11.4240903176416]
Robot Arm 1 end: [3.14843097878915;11.4081633657288;0.95992607499068]
Robot Arm 1 togoal: 7.01724490568217
Mobile Platform 2 Pose: [-0.805042344429537;3.54460581378541;5.72252290829897]
Robot Arm 2 end: [4.25079757643125;5.86447592818154;0.386972786539209]
Robot Arm 1 togoal: 21.3553938329673
Step 356:
Mobile Platform 1 Pose: [-5.4069131459112;2.39137127719341;11.4257747301126]
Robot Arm 1 end: [3.11915053058472;11.568624379028;0.815849241988293]
Robot Arm 1 togoal: 7.09252489983347
Mobile Platform 2 Pose: [-0.546633461534973;3.42191689488717;5.80834061735836]
Robot Arm 2 end: [4.10146398697036;6.00313474335344;0.35785639558689]
Robot Arm 1 togoal: 21.3612853366607
Step 357:
Mobile Platform 1 Pose: [-5.20397506014815;2.28175644359267;11.4428874655278]
Robot Arm 1 end: [2.98091412911552;11.7469356490493;0.654521723941271]
Robot Arm 1 togoal: 7.28492520398468
Mobile Platform 2 Pose: [-0.293445984417437;3.27989061664054;5.85276970554172]
Robot Arm 2 end: [3.96562250270789;5.9949324027911;0.336179339037887]
Robot Arm 1 togoal: 21.2668821047337
Step 358:
Mobile Platform 1 Pose: [-5.14175625187467;2.2541676530021;11.4609823807585]
Robot Arm 1 end: [2.87436429981391;11.8914427902642;0.504729569761326]
Robot Arm 1 togoal: 7.44211297602949
Mobile Platform 2 Pose: [-0.0285253494655416;3.14817259094376;5.9289203415292]
Robot Arm 2 end: [3.80425435497183;6.12434530732125;0.307157498485456]
Robot Arm 1 togoal: 21.2608881165622
Step 359:
Mobile Platform 1 Pose: [-5.05974073055664;2.21762863452126;11.4844413392709]
Robot Arm 1 end: [2.71706685419072;12.0121680693128;0.342758671659791]
Robot Arm 1 togoal: 7.64710325373591
Mobile Platform 2 Pose: [0.217243666876803;3.00755593252125;5.96859750588168]
Robot Arm 2 end: [3.68131599847758;6.11307952996612;0.311619602934558]
Robot Arm 1 togoal: 21.172442946765
Step 360:
Mobile Platform 1 Pose: [-4.91669550039776;2.1598219670709;11.5344495794076]
Robot Arm 1 end: [2.52133303089128;12.1269963302006;0.226663517893572]
Robot Arm 1 togoal: 7.88225094452317
Mobile Platform 2 Pose: [0.481542849804047;2.8753589951268;6.04343838346164]
Robot Arm 2 end: [3.5202830211893;6.24202530202775;0.287726440854626]
Robot Arm 1 togoal: 21.1689017796497
Step 361:
Mobile Platform 1 Pose: [-4.61758988772672;2.07688077528762;11.7051210543722]
Robot Arm 1 end: [2.39009277730137;12.3933392907146;0.29811957322706]
Robot Arm 1 togoal: 8.07057602880634
Mobile Platform 2 Pose: [0.723772740700821;2.73602801017506;6.08218720074297]
Robot Arm 2 end: [3.40903102005752;6.23156129979032;0.312332006447587]
Robot Arm 1 togoal: 21.0885412829895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.922210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [-5.29566447084602;2.31651085097701;11.6338649891899]
Robot Arm 1 end: [2.72218126503277;12.2741053882168;0.360478452983479]
Robot Arm 1 togoal: 7.71265266408686
Mobile Platform 2 Pose: [0.983088239628471;2.60816960702042;6.1590246850442]
Robot Arm 2 end: [3.26820178039816;6.27591427408935;0.28657858545316]
Robot Arm 1 togoal: 21.0350745226053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.552259e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [-5.18802299445861;2.25736895085322;11.6325203968621]
Robot Arm 1 end: [2.53099435969289;12.3058674757655;0.23690144182856]
Robot Arm 1 togoal: 7.92161468248932
Mobile Platform 2 Pose: [1.25539450514918;2.4738023724257;6.24047310460064]
Robot Arm 2 end: [3.12565838216654;6.21465346598614;0.273932019944478]
Robot Arm 1 togoal: 20.8986380784546

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.989405e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [-5.05295863458712;2.18151776722923;11.6274458135393]
Robot Arm 1 end: [2.36958520835864;12.3652090447691;0.259610042254253]
Robot Arm 1 togoal: 8.08755515242144
Mobile Platform 2 Pose: [1.26217468156749;2.55482287317497;6.35774053589115]
Robot Arm 2 end: [3.21774672251705;6.44136953899213;0.276148953298009]
Robot Arm 1 togoal: 21.1323712551806

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006015e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [-4.951545284971;2.12083141104369;11.6167859687213]
Robot Arm 1 end: [2.24193697895922;12.4202735407751;0.345502287407018]
Robot Arm 1 togoal: 8.21139821972652
Mobile Platform 2 Pose: [1.57250075843893;2.40535113973388;6.45055778803628]
Robot Arm 2 end: [3.0858054339418;6.54851320875154;0.30798477602277]
Robot Arm 1 togoal: 21.1320372706657
Step 366:
Mobile Platform 1 Pose: [-4.90554825728422;2.08773470373744;11.6025885687567]
Robot Arm 1 end: [2.14772126826198;12.4570619586721;0.509908011551974]
Robot Arm 1 togoal: 8.28961650251462
Mobile Platform 2 Pose: [1.84190763663058;2.2832574685087;6.54470536654372]
Robot Arm 2 end: [2.91722845415324;6.7629638603856;0.2802625216235]
Robot Arm 1 togoal: 21.1988485020208
Step 367:
Mobile Platform 1 Pose: [-4.77181460733536;1.99257327432674;11.5722253550175]
Robot Arm 1 end: [1.97130543983554;12.4314418154263;0.642371496435522]
Robot Arm 1 togoal: 8.43468041418842
Mobile Platform 2 Pose: [2.07187531734967;2.14706689517406;6.57369179932677]
Robot Arm 2 end: [2.81115957894815;6.74782918827933;0.305939847540516]
Robot Arm 1 togoal: 21.1208892675273
Step 368:
Mobile Platform 1 Pose: [-5.25259563362848;2.18800097783194;11.4661536870715]
Robot Arm 1 end: [2.28083440769547;12.3179903056219;0.767136571259166]
Robot Arm 1 togoal: 8.09486915073789
Mobile Platform 2 Pose: [2.34296237168258;2.00440600723486;6.63698603090295]
Robot Arm 2 end: [2.64314083191626;6.85248964770293;0.285233647622955]
Robot Arm 1 togoal: 21.1040881323802
Step 369:
Mobile Platform 1 Pose: [-5.06037726211564;2.05997411482336;11.4379645781007]
Robot Arm 1 end: [2.06219050785941;12.2674299240656;0.860909690357921]
Robot Arm 1 togoal: 8.28165304821137
Mobile Platform 2 Pose: [2.58111292832053;1.86600919997155;6.67133357116153]
Robot Arm 2 end: [2.52886884969723;6.85091952440056;0.306418247334977]
Robot Arm 1 togoal: 21.0333366897562
Step 370:
Mobile Platform 1 Pose: [-5.53223952021463;2.28605589402027;11.3881110812578]
Robot Arm 1 end: [2.38649461484283;12.2443395191611;0.687371747698291]
Robot Arm 1 togoal: 7.98106886263494
Mobile Platform 2 Pose: [2.85020168348168;1.72569080738605;6.73653962163248]
Robot Arm 2 end: [2.36223035200753;6.9584532711653;0.285440020925416]
Robot Arm 1 togoal: 21.0223805761451
Step 371:
Mobile Platform 1 Pose: [-5.97776347765663;2.49662391261382;11.3370640837029]
Robot Arm 1 end: [2.70573560899602;12.1682897175871;0.712819339723079]
Robot Arm 1 togoal: 7.65251130576807
Mobile Platform 2 Pose: [3.08490798442045;1.58942755706816;6.77079989046572]
Robot Arm 2 end: [2.25108361235331;6.95563387195462;0.30693420267722]
Robot Arm 1 togoal: 20.9536757022933
Step 372:
Mobile Platform 1 Pose: [-6.43454498796274;2.65143815957956;11.2271592846721]
Robot Arm 1 end: [2.94423658019891;11.9855387698945;0.884875512936576]
Robot Arm 1 togoal: 7.35736474602144
Mobile Platform 2 Pose: [3.35063390019958;1.45105456757727;6.83570691573902]
Robot Arm 2 end: [2.08656701365899;7.06234735531517;0.286462518731328]
Robot Arm 1 togoal: 20.9459514522916
Step 373:
Mobile Platform 1 Pose: [-6.95593896694553;2.83201035524793;11.1256020213902]
Robot Arm 1 end: [3.02788471116757;11.8688686544884;0.96445521968385]
Robot Arm 1 togoal: 7.23966875906609
Mobile Platform 2 Pose: [3.58386935047442;1.3162507907904;6.87090878742503]
Robot Arm 2 end: [1.97658399765569;7.0610878545912;0.30735710389515]
Robot Arm 1 togoal: 20.8804265309463
Step 374:
Mobile Platform 1 Pose: [-7.19165041376271;2.83063942705258;10.9293825520019]
Robot Arm 1 end: [3.08314242181598;11.5405981386496;1.1236441432449]
Robot Arm 1 togoal: 7.09748216331607
Mobile Platform 2 Pose: [3.84629632992837;1.17991761429107;6.93574727791656]
Robot Arm 2 end: [1.8143878038277;7.16703930298701;0.28748054904858]
Robot Arm 1 togoal: 20.8760285458084
Step 375:
Mobile Platform 1 Pose: [-7.77689480271449;3.05649104738902;10.8221862566853]
Robot Arm 1 end: [3.21877732940359;11.391494677048;1.166097298264]
Robot Arm 1 togoal: 6.9316083467088
Mobile Platform 2 Pose: [4.07795533061065;1.04665410948981;6.97190740739548]
Robot Arm 2 end: [1.70563137224663;7.16737150070596;0.307770986317245]
Robot Arm 1 togoal: 20.8137544596312
Step 376:
Mobile Platform 1 Pose: [-7.44842691638596;2.83280654879933;10.7105383946164]
Robot Arm 1 end: [3.01326979044657;11.0935429632446;1.2900788109732]
Robot Arm 1 togoal: 7.0755607430853
Mobile Platform 2 Pose: [4.33691548633402;0.912448661025401;7.03663559000626]
Robot Arm 2 end: [1.54588128689174;7.27247638175076;0.28850353841325]
Robot Arm 1 togoal: 20.8126130646048
Step 377:
Mobile Platform 1 Pose: [-7.93779761647737;3.00133752316853;10.5258838233805]
Robot Arm 1 end: [3.13113450070977;10.8436895277907;1.31530261623862]
Robot Arm 1 togoal: 6.92354220614922
Mobile Platform 2 Pose: [4.5669226796004;0.780795833761122;7.07378561497624]
Robot Arm 2 end: [1.43838159287251;7.27447936998271;0.308171038468955]
Robot Arm 1 togoal: 20.7536783500673
Step 378:
Mobile Platform 1 Pose: [-7.67469957416496;2.82315439935499;10.3505248361913]
Robot Arm 1 end: [2.91625186923385;10.4763109583864;1.38818861930666]
Robot Arm 1 togoal: 7.10098574650405
Mobile Platform 2 Pose: [4.82226281530796;0.648797237875737;7.13836201837959]
Robot Arm 2 end: [1.28119899082307;7.37864535888585;0.289528797410601]
Robot Arm 1 togoal: 20.755704914632
Step 379:
Mobile Platform 1 Pose: [-8.25224877386459;3.02440664550577;10.2022091815242]
Robot Arm 1 end: [3.07725812904939;10.2566424836549;1.39451572279367]
Robot Arm 1 togoal: 6.92865200804671
Mobile Platform 2 Pose: [5.05054193343149;0.518826247703433;7.17653307991552]
Robot Arm 2 end: [1.17498305165398;7.38240480941493;0.30855329284024]
Robot Arm 1 togoal: 20.700199761858
Step 380:
Mobile Platform 1 Pose: [-8.38183209125472;2.99113251414532;9.95443341155672]
Robot Arm 1 end: [3.0045714495522;9.81528571064299;1.39183561966285]
Robot Arm 1 togoal: 6.9990587517366
Mobile Platform 2 Pose: [5.30212148404242;0.389103552085953;7.24091203612663]
Robot Arm 2 end: [1.02048418519201;7.48552710043802;0.290554325154791]
Robot Arm 1 togoal: 20.7052825678609
Step 381:
Mobile Platform 1 Pose: [-9.04673672402287;3.21778865960014;9.84894885473648]
Robot Arm 1 end: [3.21099294810999;9.80028312164361;1.40444115201166]
Robot Arm 1 togoal: 6.79294410010542
Mobile Platform 2 Pose: [5.52859759463507;0.260885449220886;7.28013581523648]
Robot Arm 2 end: [0.915573246420464;7.49113876773238;0.308913458826044]
Robot Arm 1 togoal: 20.6533006677707
Step 382:
Mobile Platform 1 Pose: [-9.08958838294064;3.11232868422402;9.62228126897821]
Robot Arm 1 end: [3.07639040327371;9.48775260818817;1.41537050273259]
Robot Arm 1 togoal: 6.943336721562
Mobile Platform 2 Pose: [5.77629008984868;0.13349662942307;7.34426742523493]
Robot Arm 2 end: [0.763869314275374;7.59309854421877;0.291578364949797]
Robot Arm 1 togoal: 20.6613038445816
Step 383:
Mobile Platform 1 Pose: [-9.52463934378266;3.36232605974322;9.62113574001781]
Robot Arm 1 end: [3.33843382513387;9.57834945068503;1.404449674427]
Robot Arm 1 togoal: 6.6759146988446
Mobile Platform 2 Pose: [5.99901478333684;0.00757339756890443;7.38329131354544]
Robot Arm 2 end: [0.66040700634267;7.59995381914634;0.30893924778833]
Robot Arm 1 togoal: 20.6124172101191
Step 384:
Mobile Platform 1 Pose: [-9.96753387499673;3.38455219180814;9.46680348253403]
Robot Arm 1 end: [3.3005872088398;9.35572061916634;1.41537050261362]
Robot Arm 1 togoal: 6.73115036659013
Mobile Platform 2 Pose: [6.24200608486527;-0.118264718289336;7.44494362875466]
Robot Arm 2 end: [0.512831981702269;7.69559967798885;0.293687819728044]
Robot Arm 1 togoal: 20.6194126838517
Step 385:
Mobile Platform 1 Pose: [-10.308023262366;3.63422856637391;9.45408648643339]
Robot Arm 1 end: [3.59603015200258;9.42338816942001;1.40444967430175]
Robot Arm 1 togoal: 6.43093266918923
Mobile Platform 2 Pose: [6.46181175042142;-0.242262399546074;7.48407072149923]
Robot Arm 2 end: [0.408126380804169;7.70725137754851;0.308491617123223]
Robot Arm 1 togoal: 20.5753741809239
Step 386:
Mobile Platform 1 Pose: [-10.3705952936446;3.40039044464362;9.36565723320971]
Robot Arm 1 end: [3.28136247548829;9.29339477132313;1.41537050265388]
Robot Arm 1 togoal: 6.75651823986881
Mobile Platform 2 Pose: [6.69887900400882;-0.366901464251695;7.54129049564013]
Robot Arm 2 end: [0.266413618141584;7.79158869623144;0.296745364056476]
Robot Arm 1 togoal: 20.5776256241643
Step 387:
Mobile Platform 1 Pose: [-10.6782242262067;3.64288047592296;9.30484124246569]
Robot Arm 1 end: [3.59605858158993;9.29037695060591;1.40444967430175]
Robot Arm 1 togoal: 6.44419231099443
Mobile Platform 2 Pose: [6.91648877708855;-0.489214320762065;7.58092227043747]
Robot Arm 2 end: [0.158203454797286;7.81131619849171;0.307667864109563]
Robot Arm 1 togoal: 20.5403081044003
Step 388:
Mobile Platform 1 Pose: [-10.5489269695262;3.39298203878054;9.29771358650535]
Robot Arm 1 end: [3.25987966659002;9.27915725676479;1.41622700182917]
Robot Arm 1 togoal: 6.779366766186
Mobile Platform 2 Pose: [7.14600395170461;-0.61221607796913;7.63263158334603]
Robot Arm 2 end: [0.0236626889058443;7.88180394914318;0.300015435756528]
Robot Arm 1 togoal: 20.5359082945624
Step 389:
Mobile Platform 1 Pose: [-11.0052292949917;3.62889186566724;9.21496968293361]
Robot Arm 1 end: [3.58011836137298;9.21838649129244;1.40445685343745]
Robot Arm 1 togoal: 6.46833689876967
Mobile Platform 2 Pose: [7.36101576787044;-0.732677812476038;7.67259372004281]
Robot Arm 2 end: [-0.0884821328289854;7.91037737024634;0.30670049400781]
Robot Arm 1 togoal: 20.5059583357164
Step 390:
Mobile Platform 1 Pose: [-11.2528725187242;3.47345305406955;9.26157181137444]
Robot Arm 1 end: [3.33821083659527;9.29126588863679;1.41551063282989]
Robot Arm 1 togoal: 6.70021394470747
Mobile Platform 2 Pose: [7.58186008822462;-0.853294854771721;7.71878736378248]
Robot Arm 2 end: [-0.215635874212711;7.96764295078469;0.302708527095214]
Robot Arm 1 togoal: 20.4952240452704
Step 391:
Mobile Platform 1 Pose: [-11.6285243264683;3.68186733046116;9.12350121562209]
Robot Arm 1 end: [3.63787798050921;9.14484287538293;1.40445685340143]
Robot Arm 1 togoal: 6.42039505043825
Mobile Platform 2 Pose: [7.79320513349004;-0.971617270071898;7.75835071000839]
Robot Arm 2 end: [-0.330382111328662;8.00285642731218;0.305946080053607]
Robot Arm 1 togoal: 20.4714607608667
Step 392:
Mobile Platform 1 Pose: [-11.4362903278897;3.60044303659285;9.2244515641038]
Robot Arm 1 end: [3.46565110493795;9.36709693544625;1.38025135831388]
Robot Arm 1 togoal: 6.56643677740498
Mobile Platform 2 Pose: [8.00562628753194;-1.08944636904782;7.79994557007227]
Robot Arm 2 end: [-0.451341563687668;8.05020917452255;0.304396289042801]
Robot Arm 1 togoal: 20.456467284624
Step 393:
Mobile Platform 1 Pose: [-11.2890211676098;3.55565097495046;9.22089250389674]
Robot Arm 1 end: [3.35650195950938;9.5425574977415;1.30073838619165]
Robot Arm 1 togoal: 6.6628698798185
Mobile Platform 2 Pose: [8.2123697417481;-1.20550053015484;7.8382454830671]
Robot Arm 2 end: [-0.566607321264063;8.08828862692653;0.305605753338361]
Robot Arm 1 togoal: 20.4365424193293
Step 394:
Mobile Platform 1 Pose: [-11.134076054946;3.52498671745773;9.11337053847865]
Robot Arm 1 end: [3.34703528948715;9.53565795624761;1.27567486584455]
Robot Arm 1 togoal: 6.67365996993957
Mobile Platform 2 Pose: [8.41771001197776;-1.32067291372886;7.87646013362635]
Robot Arm 2 end: [-0.683267153641817;8.12966844986121;0.305191446948309]
Robot Arm 1 togoal: 20.4197105628355
Step 395:
Mobile Platform 1 Pose: [-10.8081870308583;3.36570385279314;9.01790537768811]
Robot Arm 1 end: [3.24416307743391;9.52278983666636;1.21685010685575]
Robot Arm 1 togoal: 6.77949623657952
Mobile Platform 2 Pose: [8.61954549275467;-1.43452813658031;7.91298653947542]
Robot Arm 2 end: [-0.797508170170803;8.16766562057832;0.305530822747959]
Robot Arm 1 togoal: 20.4016493279359
Step 396:
Mobile Platform 1 Pose: [-10.5513308280891;3.23155553204408;8.97091747554473]
Robot Arm 1 end: [3.20100586346354;9.57237083592608;1.11079395376814]
Robot Arm 1 togoal: 6.82476790601622
Mobile Platform 2 Pose: [8.81925701298257;-1.54741389510445;7.94875005150596]
Robot Arm 2 end: [-0.911373167330343;8.20574577105943;0.305435538730559]
Robot Arm 1 togoal: 20.3845508594983
Step 397:
Mobile Platform 1 Pose: [-10.3746064676275;3.13692031819597;8.94932800384524]
Robot Arm 1 end: [3.23335127494428;9.63052694521756;0.975644890186817]
Robot Arm 1 togoal: 6.79863644433999
Mobile Platform 2 Pose: [9.01648330285225;-1.65926954553648;7.98343957276218]
Robot Arm 2 end: [-1.02401001707999;8.24241917895851;0.305459045804773]
Robot Arm 1 togoal: 20.3674199025218
Step 398:
Mobile Platform 1 Pose: [-10.2504121147954;3.06575786092563;8.94390254025722]
Robot Arm 1 end: [3.30497649392895;9.64574379242744;0.83190136870093]
Robot Arm 1 togoal: 6.73971024088616
Mobile Platform 2 Pose: [9.21167072992413;-1.77023542559789;8.0172962884351]
Robot Arm 2 end: [-1.13590378230187;8.27843684084465;0.30536148021166]
Robot Arm 1 togoal: 20.3507059364571
Step 399:
Mobile Platform 1 Pose: [-10.1690915803472;3.01691203226984;8.94747932684479]
Robot Arm 1 end: [3.39082533957667;9.60964564311177;0.732300654926857]
Robot Arm 1 togoal: 6.66750424618104
Mobile Platform 2 Pose: [9.40491087159467;-1.88036183128022;8.05032146330094]
Robot Arm 2 end: [-1.24696801281948;8.31359836466983;0.305238011412337]
Robot Arm 1 togoal: 20.3342452833622
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000986e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.3422879655725;9.99051061011303;9.81851213264354]
Robot Arm 2 end: [9.76840433636833;9.82903203834472;1.48915542083236]
Robot Arm 1 togoal: 27.9996667062497

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.14397261032071;9.85631918018874;9.81438497457977]
Robot Arm 2 end: [9.54396272121016;9.88298960015729;1.46048296507258]
Robot Arm 1 togoal: 27.8801617032529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883650e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-1.9891283296878;9.74116283838215;9.82685855009369]
Robot Arm 2 end: [9.24596607034348;9.99407287067373;1.34437119066327]
Robot Arm 1 togoal: 27.7525018273112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.865893e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.88280324341765;9.64881462205419;9.85014740823448]
Robot Arm 2 end: [8.99406822976325;9.97806561792076;1.2039759542744]
Robot Arm 1 togoal: 27.5680655468681

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.915471e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.73493992951004;9.53291617948312;9.86233559023225]
Robot Arm 2 end: [8.82158745078628;10.0433937386228;1.12427687561875]
Robot Arm 1 togoal: 27.4981304085655

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.838758e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.59225679877146;9.46325403568053;9.85533909035797]
Robot Arm 2 end: [8.70920454762856;9.96988002123917;1.07302538670754]
Robot Arm 1 togoal: 27.3684329862316

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.50294877434788;9.41320518297989;9.85042386485568]
Robot Arm 2 end: [8.66797493537392;9.80823385277964;1.09219146029183]
Robot Arm 1 togoal: 27.2221103759036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.925864e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.43326872425493;9.37228943210438;9.8426113006534]
Robot Arm 2 end: [8.68202537203783;9.67928502222407;1.1299287110852]
Robot Arm 1 togoal: 27.1375250850259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.36615714367622;9.33370038538338;9.83134364151804]
Robot Arm 2 end: [8.68044378415858;9.58753119147308;1.11478525793936]
Robot Arm 1 togoal: 27.0701933603494

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997019e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.3045868179162;9.29830216982836;9.81655821887473]
Robot Arm 2 end: [8.64300071701371;9.48793704477718;1.04472123100476]
Robot Arm 1 togoal: 26.9734685841767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.24441799465187;9.26618039993267;9.80401158339205]
Robot Arm 2 end: [8.64511327791863;9.34352118228529;0.972639061760023]
Robot Arm 1 togoal: 26.8721558605655

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.15224961487832;9.2202394303502;9.78991028229339]
Robot Arm 2 end: [8.68673097618965;9.19236920656268;0.89606531260105]
Robot Arm 1 togoal: 26.7942436672696

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.00447085053307;9.14554484131854;9.7660594813968]
Robot Arm 2 end: [8.7495051339521;9.05310977630983;0.814075193428329]
Robot Arm 1 togoal: 26.7406932894111

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998245e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.753678152162074;9.00760200110345;9.70469815105858]
Robot Arm 2 end: [8.77672935307612;8.92770699046138;0.708709103902619]
Robot Arm 1 togoal: 26.6736475498818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993219e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.644623117709762;8.86636680366332;9.53198926622364]
Robot Arm 2 end: [8.73252482119901;8.72293511359937;0.722964533391925]
Robot Arm 1 togoal: 26.4970308912311

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.908093881487174;8.95833360476212;9.46130470038582]
Robot Arm 2 end: [8.70830078337757;8.68762218968754;0.877080264533217]
Robot Arm 1 togoal: 26.450753749781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998047e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-1.02704680122339;8.96308115851733;9.27713490622745]
Robot Arm 2 end: [8.69180284083584;8.53096133083161;0.964045106459029]
Robot Arm 1 togoal: 26.3266066943481

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.07698689700195;8.96468788649882;9.11423267944962]
Robot Arm 2 end: [8.60767753333982;8.37303731749262;0.83855101958217]
Robot Arm 1 togoal: 26.1587442371687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.1806447866621;8.99110507038551;8.93445380260915]
Robot Arm 2 end: [8.69548754930133;8.2045698478057;0.916105308700728]
Robot Arm 1 togoal: 26.101599867389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.17867885240295;8.98368843639022;8.77199807580693]
Robot Arm 2 end: [8.66633440511629;8.03673368989955;0.817556486298708]
Robot Arm 1 togoal: 25.9663365745142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999300e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.2237415509054;8.98826614863225;8.59107464147614]
Robot Arm 2 end: [8.72081362310262;7.83659302965986;0.793807000408665]
Robot Arm 1 togoal: 25.8677738368672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.752069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.28759410683544;9.00811210911426;8.42979476512853]
Robot Arm 2 end: [8.76310260126532;7.67103585620495;0.70198680022166]
Robot Arm 1 togoal: 25.7874060362477
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.2957716202872;8.9982337910413;8.25203785823777]
Robot Arm 2 end: [8.7954084977689;7.47901403922711;0.672212622475789]
Robot Arm 1 togoal: 25.6808051352318
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.33567614208481;9.0066311984931;8.08509329782896]
Robot Arm 2 end: [8.83222719654289;7.32269767101581;0.586661483862014]
Robot Arm 1 togoal: 25.6047187886601
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.25409898712441;8.95659574616018;7.91161238603719]
Robot Arm 2 end: [8.73580668164197;7.17300643795398;0.436410173487155]
Robot Arm 1 togoal: 25.4385325288556
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.44701627829733;9.02334657138899;7.76478275181901]
Robot Arm 2 end: [8.96592468611595;6.99996319588678;0.490365793023394]
Robot Arm 1 togoal: 25.4905326489957

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-0.739684478013078;8.78539147593563;7.68811872800223]
Robot Arm 2 end: [8.6733100714765;6.97176422182587;0.308337594906246]
Robot Arm 1 togoal: 25.2626966085468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.291019e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-0.958971211608411;8.86241836340681;7.53572429662341]
Robot Arm 2 end: [8.90607942957309;6.80259630067113;0.373062070587638]
Robot Arm 1 togoal: 25.3196533019399

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-0.929386104083104;8.8367120266087;7.32230127685581]
Robot Arm 2 end: [8.94286441481021;6.61163076631767;0.300916308907922]
Robot Arm 1 togoal: 25.2243333542757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.017870e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-1.06380937978634;8.88923115448424;7.12872257039968]
Robot Arm 2 end: [9.08927435369082;6.49338864620896;0.236613493174115]
Robot Arm 1 togoal: 25.2602833136188

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998652e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-1.47902031582072;9.06294557458697;7.0069894150204]
Robot Arm 2 end: [9.37354319451412;6.37415934412718;0.353566277605462]
Robot Arm 1 togoal: 25.393112681055

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.933001e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-1.88829225607088;9.22647468405425;6.93848530219488]
Robot Arm 2 end: [9.4601150104067;6.2189697111711;0.557450206867922]
Robot Arm 1 togoal: 25.3511238984386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-2.23621311605062;9.35357733853385;6.8655150675994]
Robot Arm 2 end: [9.39852580020117;6.10707734131318;0.684669288922464]
Robot Arm 1 togoal: 25.2277663053709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011761e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-2.50704644704602;9.37676029557336;6.70149121808195]
Robot Arm 2 end: [9.28591304806313;5.97535536200777;0.855250439101878]
Robot Arm 1 togoal: 25.0519788343699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.835380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-2.20305497339822;9.10979403471593;6.5628465293411]
Robot Arm 2 end: [9.14668403332939;5.81815908000564;0.688124835077027]
Robot Arm 1 togoal: 24.8496146286589

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.021730e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-2.28908636391163;8.9782084633183;6.40522025381243]
Robot Arm 2 end: [8.92551049716654;5.65855864898711;0.60069060644995]
Robot Arm 1 togoal: 24.5807318020027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.673410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-1.98449326173649;8.68157883475976;6.27286978629197]
Robot Arm 2 end: [8.7229465593727;5.53479675055311;0.710927507342552]
Robot Arm 1 togoal: 24.3420388502993

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.658193e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-1.56702605857454;8.35257914006467;6.14953893957151]
Robot Arm 2 end: [8.33759302497429;5.42565983996087;0.742260248285048]
Robot Arm 1 togoal: 23.9755028111752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.194218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-1.56359707274768;7.99628256080991;6.15056763531957]
Robot Arm 2 end: [7.82092970807176;5.45612143040651;0.835722370362157]
Robot Arm 1 togoal: 23.5997209890538

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.436964e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-1.18348508895375;7.81517688381725;6.10162320211746]
Robot Arm 2 end: [7.61570859912668;5.38872548451997;0.710780960608971]
Robot Arm 1 togoal: 23.4047541405385

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.395370e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-0.845457752879454;7.75804663160233;6.03014777812776]
Robot Arm 2 end: [7.54611570686878;5.31965663093776;0.594073334617155]
Robot Arm 1 togoal: 23.3113116047051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.522808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [-0.914572237574966;7.80910569479095;6.03082474136213]
Robot Arm 2 end: [7.67593495622034;5.33171273544283;0.442508192352711]
Robot Arm 1 togoal: 23.4235615685966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-1.11029783007466;7.94025299345004;6.05123640467325]
Robot Arm 2 end: [7.74262965214034;5.39373731683574;0.357444849408742]
Robot Arm 1 togoal: 23.5185440905941

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-1.17283110662591;7.96906205918469;6.06121297740529]
Robot Arm 2 end: [7.89609197256562;5.3929730556036;0.316052803935008]
Robot Arm 1 togoal: 23.6361085021704

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-1.23934469083711;7.99800663102413;6.07253009617109]
Robot Arm 2 end: [8.03990673736284;5.38834073846351;0.320832998635346]
Robot Arm 1 togoal: 23.74193898162

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-1.27618470205134;8.01282032383307;6.07967427775919]
Robot Arm 2 end: [8.18665834747955;5.40589519485798;0.333796978692672]
Robot Arm 1 togoal: 23.8643164498105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.532407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307232384746;-6.24962959526582;-7.10439671995214]
Robot Arm 1 end: [-6.13859615700735;-6.93244762513974;1.24046708975298]
Robot Arm 1 togoal: 23.3931777920573
Mobile Platform 2 Pose: [-1.32785095890216;8.03491171937186;6.09154898049562]
Robot Arm 2 end: [8.31853679415885;5.43769273359696;0.371942813215357]
Robot Arm 1 togoal: 23.9835668663859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.726064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407579755358;-6.21793445967175;-7.10337745816486]
Robot Arm 1 end: [-6.10319223959096;-6.88091550052145;1.17440882261183]
Robot Arm 1 togoal: 23.3323430860514
Mobile Platform 2 Pose: [-1.40921994297774;8.07103785197605;6.11383026833108]
Robot Arm 2 end: [8.44658157724615;5.49853918515025;0.39220292548982]
Robot Arm 1 togoal: 24.1196034709333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.011136e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859415550118;-6.1897814345502;-7.1021912417756]
Robot Arm 1 end: [-6.07854106248849;-6.82859183675456;1.1028113589657]
Robot Arm 1 togoal: 23.2786569124101
Mobile Platform 2 Pose: [-1.51533034420529;8.11702477816615;6.15245314850107]
Robot Arm 2 end: [8.57202117896878;5.59027694505833;0.397791194306385]
Robot Arm 1 togoal: 24.2742312734486

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000080e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70846313559302;-6.1635543936516;-7.10107974810431]
Robot Arm 1 end: [-6.05863254524187;-6.77629524086148;1.02307341483517]
Robot Arm 1 togoal: 23.2286825307822
Mobile Platform 2 Pose: [-1.65151929511246;8.17082390295668;6.2154274797131]
Robot Arm 2 end: [8.69312535560859;5.73548317062382;0.359612151505058]
Robot Arm 1 togoal: 24.4619538857409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000146e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74538499953029;-6.13815519369832;-7.10000572414717]
Robot Arm 1 end: [-6.04165406275177;-6.72114496266098;0.931014629999165]
Robot Arm 1 togoal: 23.179288959503
Mobile Platform 2 Pose: [-1.76180071095288;8.19154248558273;6.30602338697212]
Robot Arm 2 end: [8.75012391913305;5.88726555653081;0.332763114612188]
Robot Arm 1 togoal: 24.604557722958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990478e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78015285817028;-6.11349025174596;-7.09883317604021]
Robot Arm 1 end: [-6.02901554228635;-6.65903972144147;0.819203071743093]
Robot Arm 1 togoal: 23.1288880542047
Mobile Platform 2 Pose: [-1.80148926645936;8.16918520467792;6.40744867999599]
Robot Arm 2 end: [8.76350026256213;6.0507638512033;0.321918707454272]
Robot Arm 1 togoal: 24.7211196804756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80662729169287;-6.09323997327094;-7.09791179553062]
Robot Arm 1 end: [-5.99692343333194;-6.60121687138273;0.729564578910729]
Robot Arm 1 togoal: 23.067906948146
Mobile Platform 2 Pose: [-1.74956321950253;8.10234087179105;6.50761037198271]
Robot Arm 2 end: [8.70993775305512;6.17644855113741;0.319165877908912]
Robot Arm 1 togoal: 24.7625455821451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82840988464578;-6.07552735907972;-7.0984388769549]
Robot Arm 1 end: [-5.97854955729998;-6.56318158620984;0.646811311085405]
Robot Arm 1 togoal: 23.030788872218
Mobile Platform 2 Pose: [-1.6594834101603;8.04582681055187;6.58735659527714]
Robot Arm 2 end: [8.68087585897156;6.31116335775662;0.320298046764505]
Robot Arm 1 togoal: 24.8288311871951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986314e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85381337887741;-6.05635336521131;-7.10075035285351]
Robot Arm 1 end: [-5.94934962299944;-6.53390726607946;0.572839620259861]
Robot Arm 1 togoal: 22.9924085512087
Mobile Platform 2 Pose: [-1.55612424182437;7.99090403865998;6.65575563133339]
Robot Arm 2 end: [8.64169966764727;6.41991349882424;0.319631276065823]
Robot Arm 1 togoal: 24.8710637612965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985624e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89401362467315;-6.02981769006225;-7.10427363922201]
Robot Arm 1 end: [-5.91512327717842;-6.50986248648989;0.512046419427886]
Robot Arm 1 togoal: 22.9539690593662
Mobile Platform 2 Pose: [-1.46729548493842;7.95272429216198;6.71390860634603]
Robot Arm 2 end: [8.61633512348368;6.516397080434;0.320001070513906]
Robot Arm 1 togoal: 24.915892606468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983908e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94425327851784;-5.9989165367205;-7.10841950391043]
Robot Arm 1 end: [-5.88643385095069;-6.48947680915898;0.463221152009455]
Robot Arm 1 togoal: 22.9216169157344
Mobile Platform 2 Pose: [-1.39776336925237;7.93049362926409;6.76503465168102]
Robot Arm 2 end: [8.60488249500747;6.60908637704603;0.319574564941188]
Robot Arm 1 togoal: 24.968917197372

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981778e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99654221336589;-5.96830955732734;-7.11345407724757]
Robot Arm 1 end: [-5.86077299343159;-6.4727431236915;0.425400991524532]
Robot Arm 1 togoal: 22.8935738397409
Mobile Platform 2 Pose: [-1.34256660969651;7.91903760340258;6.81191160034706]
Robot Arm 2 end: [8.60035457449453;6.68989794732801;0.319511875459299]
Robot Arm 1 togoal: 25.019381637094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966521e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.04710295948595;-5.94056845866449;-7.11962761455545]
Robot Arm 1 end: [-5.80984763751804;-6.46899411343754;0.389420187588665]
Robot Arm 1 togoal: 22.8573734777606
Mobile Platform 2 Pose: [-1.30872650605308;7.92159980180855;6.85396898020093]
Robot Arm 2 end: [8.60318263953181;6.73446308204384;0.319142950612083]
Robot Arm 1 togoal: 25.0512498556705

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.743841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.07963467319098;-5.92434597965383;-7.12418363002373]
Robot Arm 1 end: [-5.75921469734872;-6.47610976354889;0.374766126519633]
Robot Arm 1 togoal: 22.8282476968855
Mobile Platform 2 Pose: [-1.31903357301806;7.94759105779462;6.88143184103051]
Robot Arm 2 end: [8.62947123114732;6.76574716727197;0.318426751952513]
Robot Arm 1 togoal: 25.0917049808488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.828538e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.12067136079534;-5.89723820506609;-7.12350076438434]
Robot Arm 1 end: [-5.75867237095314;-6.48371705570588;0.381307667508757]
Robot Arm 1 togoal: 22.8330370621245
Mobile Platform 2 Pose: [-1.34698343682552;7.97774822693728;6.89597851310458]
Robot Arm 2 end: [8.65976517405353;6.78290665702076;0.317848435597367]
Robot Arm 1 togoal: 25.125691351603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.414960e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.16666762477033;-5.86918472813086;-7.12403643092774]
Robot Arm 1 end: [-5.64056922576134;-6.55436535186658;0.347518245191407]
Robot Arm 1 togoal: 22.8046371893477
Mobile Platform 2 Pose: [-1.37622122011361;8.00367507248811;6.90312817606524]
Robot Arm 2 end: [8.68575704631845;6.79202177835363;0.317361609349938]
Robot Arm 1 togoal: 25.1511084681011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.213344e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.1985413589945;-5.84912918674076;-7.12842431024547]
Robot Arm 1 end: [-5.65348243781353;-6.56081644916853;0.349685526355339]
Robot Arm 1 togoal: 22.8180659016087
Mobile Platform 2 Pose: [-1.4037145990227;8.0258975156332;6.90686851235199]
Robot Arm 2 end: [8.70800827106182;6.79736717531333;0.31693048349431]
Robot Arm 1 togoal: 25.171231612557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.581448e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.22163045143315;-5.83422209613968;-7.13217574244315]
Robot Arm 1 end: [-5.6768739759275;-6.569628147688;0.335145644788456]
Robot Arm 1 togoal: 22.8412610238914
Mobile Platform 2 Pose: [-1.43065225098681;8.04645894085543;6.90890905536995]
Robot Arm 2 end: [8.72857790056637;6.80085606924749;0.316518441355447]
Robot Arm 1 togoal: 25.1888699758895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.587146e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.24274617269994;-5.82089980353191;-7.13532904946826]
Robot Arm 1 end: [-5.71973524589349;-6.58022293475076;0.316611324001814]
Robot Arm 1 togoal: 22.8793448518504
Mobile Platform 2 Pose: [-1.45825256251102;8.06679936484951;6.91004478747722]
Robot Arm 2 end: [8.74891032632775;6.80336723765377;0.316107323139579]
Robot Arm 1 togoal: 25.2056849799978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.25993684529507;-5.81000809955391;-7.13785470505567]
Robot Arm 1 end: [-5.7649325640362;-6.59393612454421;0.294544589393741]
Robot Arm 1 togoal: 22.9215184429014
Mobile Platform 2 Pose: [-1.48622175111518;8.08752199159185;6.91131583866009]
Robot Arm 2 end: [8.76955118821316;6.83522744394555;0.309287694320756]
Robot Arm 1 togoal: 25.2426064741355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.915080e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998268e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.27463231207807;-5.800647804416;-7.14001006687947]
Robot Arm 1 end: [-5.81364726178763;-6.61075312706692;0.271288891667266]
Robot Arm 1 togoal: 22.9684639674249
Mobile Platform 2 Pose: [-1.52463583390244;8.11665126890797;6.91553057296955]
Robot Arm 2 end: [8.79626613149717;6.85874834761519;0.303277659593331]
Robot Arm 1 togoal: 25.2784466301355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997780e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.28805393929877;-5.79204528430824;-7.14193587712702]
Robot Arm 1 end: [-5.88478812236785;-6.64181477604379;0.247742774492957]
Robot Arm 1 togoal: 23.0412169100283
Mobile Platform 2 Pose: [-1.56735863322461;8.15238350914768;6.92435090036651]
Robot Arm 2 end: [8.8300880636477;6.86975906582394;0.30040542958834]
Robot Arm 1 togoal: 25.3110813385955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.29937959206778;-5.78477917220312;-7.14341046973818]
Robot Arm 1 end: [-5.95206427629958;-6.68083725097195;0.224020719801529]
Robot Arm 1 togoal: 23.1171114398376
Mobile Platform 2 Pose: [-1.61255011516249;8.19240164591806;6.93612821241082]
Robot Arm 2 end: [8.8702654192951;6.8733051659047;0.300353627242569]
Robot Arm 1 togoal: 25.3433486694974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969845e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998021e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.31144294123711;-5.77702618564952;-7.14477453417495]
Robot Arm 1 end: [-6.04231514468671;-6.72689049030484;0.217904574562894]
Robot Arm 1 togoal: 23.2129877086921
Mobile Platform 2 Pose: [-1.66234981788051;8.23650481524253;6.94829490887929]
Robot Arm 2 end: [8.91420438482705;6.87931590512374;0.29993656618462]
Robot Arm 1 togoal: 25.3800990708345

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956718e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998283e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.32378315445247;-5.76916078524267;-7.14587202705192]
Robot Arm 1 end: [-6.13783085420793;-6.77721600813609;0.232382177879946]
Robot Arm 1 togoal: 23.3144825710151
Mobile Platform 2 Pose: [-1.71722851942625;8.28582776257528;6.96183409676099]
Robot Arm 2 end: [8.96308603788985;6.88805206489423;0.299074001834533]
Robot Arm 1 togoal: 25.4223924478398

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999018e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.34281515043316;-5.75698099685426;-7.14731098233617]
Robot Arm 1 end: [-6.17846054480938;-6.77260499398936;0.290377696038183]
Robot Arm 1 togoal: 23.3361799602968
Mobile Platform 2 Pose: [-1.77894529348142;8.3426744976865;6.97814819818876]
Robot Arm 2 end: [9.01958275207989;6.89918307272962;0.298617295078393]
Robot Arm 1 togoal: 25.4719676520315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982026e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999788e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.3571460956098;-5.74764064179572;-7.14854429527156]
Robot Arm 1 end: [-6.22158922528959;-6.77266736249426;0.34678791980031]
Robot Arm 1 togoal: 23.3632425287287
Mobile Platform 2 Pose: [-1.84857693824064;8.40829710241716;6.99782800074822]
Robot Arm 2 end: [9.0846569188767;6.91477623866223;0.297778310633735]
Robot Arm 1 togoal: 25.5309626966273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000074e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.36928944361344;-5.73949789219433;-7.14972733382896]
Robot Arm 1 end: [-6.26322538592036;-6.78182019404765;0.400442238282196]
Robot Arm 1 togoal: 23.3961030660223
Mobile Platform 2 Pose: [-1.9277386798161;8.48534761320695;7.02300596280128]
Robot Arm 2 end: [9.15949989344909;6.93591098523212;0.294188828234105]
Robot Arm 1 togoal: 25.6011051073572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000169e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.38013927407057;-5.73191960155682;-7.15096405746166]
Robot Arm 1 end: [-6.25374931130397;-6.82378211957372;0.3307957548115]
Robot Arm 1 togoal: 23.4230783123582
Mobile Platform 2 Pose: [-2.01285001869857;8.5760305543905;7.0592436841142]
Robot Arm 2 end: [9.2446297261569;6.95273269765758;0.288668600223855]
Robot Arm 1 togoal: 25.6762450505128

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002951e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.41076355396584;-5.71292388712524;-7.15310683362894]
Robot Arm 1 end: [-6.2269496205449;-6.86912019874255;0.282425256771223]
Robot Arm 1 togoal: 23.4396070287428
Mobile Platform 2 Pose: [-2.1072896688738;8.68389908798741;7.10594816991701]
Robot Arm 2 end: [9.3531382393879;7.02807675256944;0.282263325675963]
Robot Arm 1 togoal: 25.8076311612006

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996122e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.4278260691581;-5.70074829149941;-7.15567242356535]
Robot Arm 1 end: [-6.20678814550238;-6.92103711362812;0.24510222319805]
Robot Arm 1 togoal: 23.4650675287932
Mobile Platform 2 Pose: [-2.07768208080025;8.74520092627;7.20855511482595]
Robot Arm 2 end: [9.3565878030369;7.11409510574893;0.194883273725878]
Robot Arm 1 togoal: 25.8713804857221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.43777990445501;-5.6931817191015;-7.15808172620638]
Robot Arm 1 end: [-6.19569624555882;-6.97401591475783;0.217141245243698]
Robot Arm 1 togoal: 23.4971879339894
Mobile Platform 2 Pose: [-2.01963100794643;8.82767869496753;7.34545285114692]
Robot Arm 2 end: [9.35386619535933;7.20328803599262;0.108493719537441]
Robot Arm 1 togoal: 25.9329988614804

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997566e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.44812877246138;-5.68646066351417;-7.16002375833812]
Robot Arm 1 end: [-6.20872326050521;-7.03705103452593;0.204756569391322]
Robot Arm 1 togoal: 23.5524163188685
Mobile Platform 2 Pose: [-1.99226426048347;9.00653289966383;7.49646559760795]
Robot Arm 2 end: [9.55226274264432;7.36625683300522;0.143679674941504]
Robot Arm 1 togoal: 26.1873035271888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.45843437750018;-5.68032237062737;-7.16148700666878]
Robot Arm 1 end: [-6.22785757703669;-7.09012922856753;0.212481332673847]
Robot Arm 1 togoal: 23.6035611654781
Mobile Platform 2 Pose: [-1.961513945513;9.02222089492879;7.52934779837982]
Robot Arm 2 end: [9.55459574646063;7.32144124620341;0.123917684500201]
Robot Arm 1 togoal: 26.1603971806518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999211e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.074513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.4646950111745;-5.67722105452299;-7.16228953414657]
Robot Arm 1 end: [-6.25250602421956;-7.13437406680983;0.230685359022157]
Robot Arm 1 togoal: 23.6515462200969
Mobile Platform 2 Pose: [-1.5441787508093;8.87827895485807;7.69862561632781]
Robot Arm 2 end: [9.30196231585198;7.41854204199739;0.225099423130249]
Robot Arm 1 togoal: 26.0317251525236

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999520e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.014495e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.46223200269421;-5.68007070489726;-7.16201875003964]
Robot Arm 1 end: [-6.28874542833079;-7.16655896080785;0.257398699062811]
Robot Arm 1 togoal: 23.6983261525279
Mobile Platform 2 Pose: [-1.54079070788499;8.88309029876245;7.68692978224931]
Robot Arm 2 end: [9.36170500155448;7.33424655890368;0.259066743597233]
Robot Arm 1 togoal: 26.018151353924

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999292e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.44690562875884;-5.69185989034243;-7.15950390310822]
Robot Arm 1 end: [-6.34295012524053;-7.17831067166611;0.29083238778452]
Robot Arm 1 togoal: 23.7423606383721
Mobile Platform 2 Pose: [-1.56975696190507;8.91111842979483;7.67249320962872]
Robot Arm 2 end: [9.38795236944188;7.21526459966996;0.306846211428518]
Robot Arm 1 togoal: 25.9563518572757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997177e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.41141966425476;-5.71807678383508;-7.15092308327062]
Robot Arm 1 end: [-6.40083273392822;-7.15288909221082;0.297911806546211]
Robot Arm 1 togoal: 23.7635195900132
Mobile Platform 2 Pose: [-1.59884828738918;8.94257039202962;7.65072468743937]
Robot Arm 2 end: [9.39150677549528;7.09134983152092;0.364240815936508]
Robot Arm 1 togoal: 25.8743669661197

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.3633465262908;-5.75746051460964;-7.12822929586601]
Robot Arm 1 end: [-6.44336983085089;-7.10609423953246;0.268616198964995]
Robot Arm 1 togoal: 23.7607099370157
Mobile Platform 2 Pose: [-1.55624172880782;8.9422730867257;7.61091900015231]
Robot Arm 2 end: [9.27868193345948;6.95675484880278;0.33894911659414]
Robot Arm 1 togoal: 25.7020691070749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996245e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.30738683174742;-5.80951421682616;-7.08304300058924]
Robot Arm 1 end: [-6.48401921490614;-7.02850208257958;0.234937121671061]
Robot Arm 1 togoal: 23.7349263824607
Mobile Platform 2 Pose: [-1.41105501370708;8.88147786581133;7.5622548924269]
Robot Arm 2 end: [9.06616024717095;6.85553620762349;0.291092702016658]
Robot Arm 1 togoal: 25.4782306956143

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997836e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986095e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-4.21535057975962;-5.88778070926353;-7.01460378730457]
Robot Arm 1 end: [-6.57783610097093;-6.91479775913154;0.264312807107355]
Robot Arm 1 togoal: 23.7173838296671
Mobile Platform 2 Pose: [-1.20621345312886;8.77969884058413;7.52315419871224]
Robot Arm 2 end: [8.85041760412963;6.81494877381559;0.253548438179845]
Robot Arm 1 togoal: 25.2920378692126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985821e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-4.22387247058138;-5.91563252203522;-6.92096699463614]
Robot Arm 1 end: [-6.61984721670913;-6.82459993497791;0.283129406428863]
Robot Arm 1 togoal: 23.6816132914386
Mobile Platform 2 Pose: [-0.995476718333519;8.6713367758909;7.48949249149275]
Robot Arm 2 end: [8.61314334174816;6.80882669968646;0.206972213252021]
Robot Arm 1 togoal: 25.1139887135616

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-4.24577794607924;-5.93112029294672;-6.82367846578733]
Robot Arm 1 end: [-6.63091574140861;-6.73796806502502;0.26226462026131]
Robot Arm 1 togoal: 23.6290361229534
Mobile Platform 2 Pose: [-0.779472085336959;8.55478760130672;7.46024886361705]
Robot Arm 2 end: [8.37335710227039;6.80950682810952;0.195127938096186]
Robot Arm 1 togoal: 24.9378769693081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001360e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-4.30123147060185;-5.92537913374548;-6.72230833111987]
Robot Arm 1 end: [-6.62229366817146;-6.65856479783526;0.243746202242149]
Robot Arm 1 togoal: 23.567770475344
Mobile Platform 2 Pose: [-0.568345700093916;8.43618563108396;7.43560314030114]
Robot Arm 2 end: [8.1375107305603;6.83075940805915;0.194176333004813]
Robot Arm 1 togoal: 24.779108514966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974943e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-4.38516208085726;-5.89985795659581;-6.62016098179617]
Robot Arm 1 end: [-6.59869359720281;-6.53832297564127;0.242274455494006]
Robot Arm 1 togoal: 23.4663140479841
Mobile Platform 2 Pose: [-0.356503686245079;8.3127032539905;7.41350927402093]
Robot Arm 2 end: [7.9107104325463;6.8747550631439;0.193200247132178]
Robot Arm 1 togoal: 24.6437407635963

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.887057e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-4.55965868071553;-5.81225030856654;-6.5453315001098]
Robot Arm 1 end: [-6.48352915922209;-6.47977008826227;0.190654406918395]
Robot Arm 1 togoal: 23.3465067001884
Mobile Platform 2 Pose: [-0.129147948685222;8.17561417555356;7.3916851824405]
Robot Arm 2 end: [7.6645442746823;6.91821941693085;0.245200531095936]
Robot Arm 1 togoal: 24.4926506723238

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.256561e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-4.74145927865437;-5.7175876386653;-6.46694544487644]
Robot Arm 1 end: [-6.34945805925125;-6.35662268150016;0.141701652219563]
Robot Arm 1 togoal: 23.1677868668626
Mobile Platform 2 Pose: [0.121410478627765;8.01769586584862;7.36900812375719]
Robot Arm 2 end: [7.38718409628976;6.97440636163777;0.383797224824588]
Robot Arm 1 togoal: 24.3256628333155

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.465495e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-4.95008154164659;-5.60504151677731;-6.40527734146126]
Robot Arm 1 end: [-6.14060231822049;-6.32846023286483;0.037033806773311]
Robot Arm 1 togoal: 23.0073860408746
Mobile Platform 2 Pose: [0.433258984677753;7.79573511993339;7.33823642661639]
Robot Arm 2 end: [7.08972467047161;7.07253114524685;0.521863217368479]
Robot Arm 1 togoal: 24.1770197362776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.803055e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-5.13647392277861;-5.51066972489141;-6.27573215673825]
Robot Arm 1 end: [-6.08644280367114;-6.17073506825281;0.084044591498078]
Robot Arm 1 togoal: 22.8545653113208
Mobile Platform 2 Pose: [0.664623344059802;7.52672794934;7.3000704825858]
Robot Arm 2 end: [6.79629581713856;7.09899209105072;0.634858172266357]
Robot Arm 1 togoal: 23.9849188250763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.386592e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-5.38707520848263;-5.38013361394623;-6.22917159225389]
Robot Arm 1 end: [-6.00943883513343;-6.02816125253423;0.184561831067169]
Robot Arm 1 togoal: 22.6933944559314
Mobile Platform 2 Pose: [0.772599721722559;7.27230016560301;7.28574722993464]
Robot Arm 2 end: [6.5114278035609;7.23611341524095;0.659764233787079]
Robot Arm 1 togoal: 23.8841491542039

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.801232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-5.49468594236473;-5.32762492419853;-6.21753369329791]
Robot Arm 1 end: [-5.94485203376291;-5.94993086855929;0.214704222893728]
Robot Arm 1 togoal: 22.5908169340487
Mobile Platform 2 Pose: [0.856895181084634;7.04395011229402;7.30495813994388]
Robot Arm 2 end: [6.27730649981121;7.38214917585955;0.737604358753721]
Robot Arm 1 togoal: 23.8265298688532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.092223e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-5.56680843507903;-5.29505413513685;-6.20869202817661]
Robot Arm 1 end: [-5.87737490695131;-5.87391839206444;0.228366226508537]
Robot Arm 1 togoal: 22.4887354274574
Mobile Platform 2 Pose: [0.82887378508004;6.86670041140941;7.34170320889314]
Robot Arm 2 end: [6.08478865520184;7.33515069216871;0.655318268300805]
Robot Arm 1 togoal: 23.6638390897019

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012057e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.435788e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-5.65532002805764;-5.25877550923168;-6.19693454674581]
Robot Arm 1 end: [-5.78160090245325;-5.74008884898554;0.289537384299049]
Robot Arm 1 togoal: 22.3232126763184
Mobile Platform 2 Pose: [1.04040259186337;6.62831468485429;7.36177958352205]
Robot Arm 2 end: [5.87775490588197;7.48316912710664;0.739257477217619]
Robot Arm 1 togoal: 23.6299687885998

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010701e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.714985e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-5.68335538193422;-5.27376198660395;-6.17049969767718]
Robot Arm 1 end: [-5.70348766855195;-5.64265501001789;0.273769206777945]
Robot Arm 1 togoal: 22.20017489504
Mobile Platform 2 Pose: [0.981255675179618;6.49261399616387;7.41230792986474]
Robot Arm 2 end: [5.71432756500036;7.42509573854202;0.655851122101567]
Robot Arm 1 togoal: 23.4802584078288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.365661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-5.62233885553856;-5.31547682353506;-6.16310254162087]
Robot Arm 1 end: [-5.64099628176974;-5.56089946181246;0.267749119438269]
Robot Arm 1 togoal: 22.098710245546
Mobile Platform 2 Pose: [1.17663812783967;6.28166780510849;7.43695355571843]
Robot Arm 2 end: [5.53755030952911;7.57670659534952;0.740041057370658]
Robot Arm 1 togoal: 23.4726645532676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.477411e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-5.59781806884826;-5.32809743787295;-6.16489108491146]
Robot Arm 1 end: [-5.6571181785997;-5.62951589261239;0.165295634232164]
Robot Arm 1 togoal: 22.1644997918614
Mobile Platform 2 Pose: [1.22855907034138;6.14537977963572;7.48868804817446]
Robot Arm 2 end: [5.4256898792416;7.74170762582329;0.702160928433964]
Robot Arm 1 togoal: 23.5242554644319

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.825364e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-5.56213213333216;-5.34623184982553;-6.16818728272757]
Robot Arm 1 end: [-5.69856653021042;-5.6162954163436;0.184356485632306]
Robot Arm 1 togoal: 22.1833335979617
Mobile Platform 2 Pose: [1.2340583497908;6.04502988374588;7.54621597052866]
Robot Arm 2 end: [5.3314807505871;7.85208764036508;0.673431545717633]
Robot Arm 1 togoal: 23.5471804556213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.640844e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-5.5551968637621;-5.34857303229583;-6.17050457430256]
Robot Arm 1 end: [-5.74622859225533;-5.63785695052384;0.187164547561322]
Robot Arm 1 togoal: 22.2320804671715
Mobile Platform 2 Pose: [1.29190932982162;5.92330508309408;7.60331714973906]
Robot Arm 2 end: [5.27855169674722;8.02644510680966;0.678515631688106]
Robot Arm 1 togoal: 23.6452245717787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.370969e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-5.55701210760828;-5.34605165883375;-6.17234780455702]
Robot Arm 1 end: [-5.75473027906108;-5.67650357196533;0.149038500507139]
Robot Arm 1 togoal: 22.267612551953
Mobile Platform 2 Pose: [1.25622450813094;5.86783909740884;7.66255345154165]
Robot Arm 2 end: [5.1989672083353;8.07394796671503;0.582109993932515]
Robot Arm 1 togoal: 23.633825626569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.749885e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-5.54436526572664;-5.35074296814458;-6.17471536374413]
Robot Arm 1 end: [-5.81347020111408;-5.6397409747047;0.242071211855686]
Robot Arm 1 togoal: 22.2778588828129
Mobile Platform 2 Pose: [1.3533387362578;5.74976394500804;7.71159444742841]
Robot Arm 2 end: [5.18315364045271;8.24007933053815;0.678785142544644]
Robot Arm 1 togoal: 23.7473782620321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-5.55247333409229;-5.34442652042816;-6.17546554513245]
Robot Arm 1 end: [-5.80671496749427;-5.6406991696823;0.241557369815223]
Robot Arm 1 togoal: 22.2737666779564
Mobile Platform 2 Pose: [1.29136432111725;5.72927283880798;7.76341156129248]
Robot Arm 2 end: [5.1046096405796;8.2385296248728;0.530549771396438]
Robot Arm 1 togoal: 23.7017760289602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-5.55591939501596;-5.34063424416449;-6.17638705467966]
Robot Arm 1 end: [-5.78885902597429;-5.6326461421355;0.241910293737474]
Robot Arm 1 togoal: 22.2554210300318
Mobile Platform 2 Pose: [1.39271808550148;5.62340434326517;7.80644536380755]
Robot Arm 2 end: [5.10764372212501;8.39121115120568;0.645582769204478]
Robot Arm 1 togoal: 23.81688270473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-5.54719847893243;-5.34386984114831;-6.17783540352472]
Robot Arm 1 end: [-5.7568125701492;-5.61217246730096;0.243468584029538]
Robot Arm 1 togoal: 22.2182168059985
Mobile Platform 2 Pose: [1.33903616869609;5.60547625512092;7.85663407403278]
Robot Arm 2 end: [5.04520936863922;8.39861764625076;0.497091857591041]
Robot Arm 1 togoal: 23.7889857733198

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-5.50882427828332;-5.36555069097173;-6.17941704635161]
Robot Arm 1 end: [-5.70466483226681;-5.57474819270815;0.246541278823144]
Robot Arm 1 togoal: 22.1547630046473
Mobile Platform 2 Pose: [1.42861663468453;5.51481932623719;7.89788538412872]
Robot Arm 2 end: [5.07247947114729;8.5405451263325;0.597033816806328]
Robot Arm 1 togoal: 23.9120297025872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-5.41362332375059;-5.42576421894159;-6.17246070532102]
Robot Arm 1 end: [-5.63655501914896;-5.55976289053309;0.19455779261262]
Robot Arm 1 togoal: 22.0990389611409
Mobile Platform 2 Pose: [1.38783714033672;5.49709510917473;7.94674731461471]
Robot Arm 2 end: [5.03894857910582;8.56830944269549;0.466305279499996]
Robot Arm 1 togoal: 23.9178692796439

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-5.24882791885657;-5.52609755736937;-6.13998380513975]
Robot Arm 1 end: [-5.61420838755988;-5.55467270628033;0.141244354599692]
Robot Arm 1 togoal: 22.0829135806547
Mobile Platform 2 Pose: [1.46292820385733;5.42298340855241;7.98670518030552]
Robot Arm 2 end: [5.09381037786966;8.68991914702125;0.546316092653061]
Robot Arm 1 togoal: 24.0434231790009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-5.03007066123618;-5.64181217186908;-6.07749334882985]
Robot Arm 1 end: [-5.61913133697378;-5.60277996346001;0.042091302204755]
Robot Arm 1 togoal: 22.1267073295192
Mobile Platform 2 Pose: [1.43783980172412;5.40492979217724;8.03407262153448]
Robot Arm 2 end: [5.10000005869867;8.73460621336885;0.443149570350223]
Robot Arm 1 togoal: 24.0864533149112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-4.71259461224173;-5.78063705762996;-5.95789961513956]
Robot Arm 1 end: [-5.65120611188165;-5.47807945056488;0.0637717270613191]
Robot Arm 1 togoal: 22.0602518225439
Mobile Platform 2 Pose: [1.49931213874647;5.34761399626592;8.0715669024587]
Robot Arm 2 end: [5.18210541020278;8.82255617265317;0.504833598174686]
Robot Arm 1 togoal: 24.2036678979735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.864298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-4.6124233965315;-5.71773732774819;-5.69162775810444]
Robot Arm 1 end: [-5.67002606970837;-5.17885057216116;0.0807914718410168]
Robot Arm 1 togoal: 21.8637010206854
Mobile Platform 2 Pose: [1.49403047949233;5.3276423516916;8.11704698665769]
Robot Arm 2 end: [5.23787796768212;8.86920945612529;0.44072012439169]
Robot Arm 1 togoal: 24.2777057208202

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.730776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-4.48682919054337;-5.75599853398897;-5.60899627059698]
Robot Arm 1 end: [-5.71350102781471;-5.09313290794908;0.0522579316274951]
Robot Arm 1 togoal: 21.8374444198848
Mobile Platform 2 Pose: [1.54793974017378;5.28317974234857;8.15185259447654]
Robot Arm 2 end: [5.33789284623761;8.91122628582822;0.494603117131069]
Robot Arm 1 togoal: 24.3708622571634

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.841290e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-4.21666863130373;-5.8493341451669;-5.45347352203737]
Robot Arm 1 end: [-5.75711587302224;-5.12776752262988;-0.0461459074925306]
Robot Arm 1 togoal: 21.8995889721692
Mobile Platform 2 Pose: [1.5653153605103;5.25873350752946;8.19855538118258]
Robot Arm 2 end: [5.44148421558221;8.93118392360589;0.476852550536641]
Robot Arm 1 togoal: 24.4523905462368

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001733e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-4.39228241273177;-5.73873488738158;-5.42602906490425]
Robot Arm 1 end: [-5.74876131218249;-4.96369970519391;-0.0133196265992759]
Robot Arm 1 togoal: 21.7781984666291
Mobile Platform 2 Pose: [1.63297174059431;5.21265094131336;8.23862656210522]
Robot Arm 2 end: [5.53266559429018;8.926676479496;0.553254400105001]
Robot Arm 1 togoal: 24.5034551557463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013162e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-3.94913332783693;-5.92276085463029;-5.23796404350539]
Robot Arm 1 end: [-5.8014954878329;-5.01571309416762;-0.1247739538214]
Robot Arm 1 togoal: 21.8601800378093
Mobile Platform 2 Pose: [1.61179649058075;5.2169082236083;8.30366885757982]
Robot Arm 2 end: [5.64454104979221;8.91436811821061;0.467965286979049]
Robot Arm 1 togoal: 24.5685544563908

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012946e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-4.3722963620147;-5.67361440298213;-5.21731729626924]
Robot Arm 1 end: [-5.58623556364545;-4.8397614961655;-0.0465615958497278]
Robot Arm 1 togoal: 21.5779171762546
Mobile Platform 2 Pose: [1.72568254362895;5.15271493467805;8.32338831555241]
Robot Arm 2 end: [5.70384538892636;8.85200985534556;0.60948613142785]
Robot Arm 1 togoal: 24.5527980710539

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958622e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-4.20393359465751;-5.65634393767329;-4.96548040031014]
Robot Arm 1 end: [-5.50555926503232;-4.71200540921615;-0.103438208511793]
Robot Arm 1 togoal: 21.4360507271164
Mobile Platform 2 Pose: [1.64990341964602;5.19723701418736;8.33483311429177]
Robot Arm 2 end: [5.78089138588476;8.81133830893116;0.473805103259813]
Robot Arm 1 togoal: 24.5764032978366

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.403529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-4.49242630933065;-5.43326205477789;-4.88228709667837]
Robot Arm 1 end: [-5.37871306410517;-4.46055523361278;-0.00353472295056861]
Robot Arm 1 togoal: 21.1645146245768
Mobile Platform 2 Pose: [1.67094134463211;5.20109378430475;8.43027415097268]
Robot Arm 2 end: [5.84696732757554;8.78690385120209;0.424465790741481]
Robot Arm 1 togoal: 24.6023681190985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.791595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-4.57122167779191;-5.36810617391487;-4.84029931828939]
Robot Arm 1 end: [-5.38286585730841;-4.33237881235884;0.014748731203244]
Robot Arm 1 togoal: 21.0788623338559
Mobile Platform 2 Pose: [1.87820692392039;5.0886901258674;8.46001756177693]
Robot Arm 2 end: [5.81789748295723;8.69114726553307;0.608729908453547]
Robot Arm 1 togoal: 24.5030019304394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015732e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-4.29160806208491;-5.49551699921849;-4.76040056528014]
Robot Arm 1 end: [-5.39928321854329;-4.35784026133585;-0.0313130236647675]
Robot Arm 1 togoal: 21.1114939390462
Mobile Platform 2 Pose: [1.79282359668732;5.1419173693179;8.46333228569302]
Robot Arm 2 end: [5.87846945731639;8.63949307730952;0.489412061186616]
Robot Arm 1 togoal: 24.5075634888804

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013499e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-4.36554982399685;-5.33784998554586;-4.48050103493163]
Robot Arm 1 end: [-5.47871590738836;-4.0296959842664;-0.00798948992919229]
Robot Arm 1 togoal: 20.9466184481623
Mobile Platform 2 Pose: [1.60269835856449;5.26953065711577;8.48473683869068]
Robot Arm 2 end: [5.95305493331455;8.65937717803709;0.292788194688169]
Robot Arm 1 togoal: 24.580089963552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990383e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-4.1780897866945;-5.43702261339441;-4.42865216292084]
Robot Arm 1 end: [-5.47221011472006;-4.04529645681059;-0.0501826999432251]
Robot Arm 1 togoal: 20.9553872183346
Mobile Platform 2 Pose: [1.86737888609518;5.13306974570711;8.53126940497615]
Robot Arm 2 end: [5.81104438769952;8.69546299291538;0.314432625480321]
Robot Arm 1 togoal: 24.5145929188826

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.653209e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-4.53806643799587;-5.20579664579179;-4.33360160286699]
Robot Arm 1 end: [-5.31643971042107;-4.01918318121475;-0.0586073748435312]
Robot Arm 1 togoal: 20.8236880002097
Mobile Platform 2 Pose: [2.04244168997839;5.02780580349371;8.53552945552273]
Robot Arm 2 end: [5.73780874745196;8.7119950447189;0.42528299959004]
Robot Arm 1 togoal: 24.4748437884687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.608188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-5.29753156936682;-4.80357840165475;-4.46146038732162]
Robot Arm 1 end: [-4.97608541484472;-4.11683904971984;-0.0115584140425076]
Robot Arm 1 togoal: 20.637757009929
Mobile Platform 2 Pose: [2.3563802331453;4.84762663654245;8.56117048455671]
Robot Arm 2 end: [5.58760445447861;8.74451338913243;0.584746004247212]
Robot Arm 1 togoal: 24.3968597645109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.930382e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-5.5686313967903;-4.64648283211366;-4.47838902976778]
Robot Arm 1 end: [-4.71820279046409;-4.26351886156727;-0.0690040635143909]
Robot Arm 1 togoal: 20.557275515435
Mobile Platform 2 Pose: [2.22055233684022;4.93535250018436;8.56936781915675]
Robot Arm 2 end: [5.65578624017166;8.75259318137516;0.425309283887352]
Robot Arm 1 togoal: 24.4533010444292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-5.79006832083956;-4.5177534441837;-4.48518443536573]
Robot Arm 1 end: [-4.55257963969422;-4.45059365623082;-0.0881226745873564]
Robot Arm 1 togoal: 20.5714974410958
Mobile Platform 2 Pose: [2.05742182800253;5.12118296241687;8.73660130657322]
Robot Arm 2 end: [5.83022108082732;8.9051730646806;0.340440512837431]
Robot Arm 1 togoal: 24.6859309886446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-6.00699833293665;-4.38529997808501;-4.48379883126748]
Robot Arm 1 end: [-4.48476434313595;-4.32020835915105;-0.0808603165796961]
Robot Arm 1 togoal: 20.4314150757972
Mobile Platform 2 Pose: [1.89466673595847;5.23787846067367;8.76737271325252]
Robot Arm 2 end: [5.95013048024487;8.94653356245231;0.304308954394788]
Robot Arm 1 togoal: 24.7963330650002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-6.2222811267685;-4.25720365654229;-4.48646788189027]
Robot Arm 1 end: [-4.38371858826829;-4.48409965814279;-0.0838847748454396]
Robot Arm 1 togoal: 20.4757456146055
Mobile Platform 2 Pose: [1.80923381790223;5.29136017108202;8.77036474999602]
Robot Arm 2 end: [5.95326361548272;8.93765146478967;0.199828570546941]
Robot Arm 1 togoal: 24.7969102295616

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-6.42594662467542;-4.12542242502393;-4.46793194584953]
Robot Arm 1 end: [-4.35493248444604;-4.35824647398873;-0.0524026309392503]
Robot Arm 1 togoal: 20.3641578337986
Mobile Platform 2 Pose: [1.61142291133286;5.43556939765705;8.81385199313684]
Robot Arm 2 end: [6.10172060491674;8.97537160844965;0.176170693632274]
Robot Arm 1 togoal: 24.9226543526331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-6.60648131774651;-4.01945907288231;-4.45642383323974]
Robot Arm 1 end: [-4.34434846694399;-4.2961452705075;-0.00773493926708962]
Robot Arm 1 togoal: 20.3095330584213
Mobile Platform 2 Pose: [1.54107361851729;5.4787140370978;8.81434981997209]
Robot Arm 2 end: [6.14444458107766;8.97566422609791;0.17043376276754]
Robot Arm 1 togoal: 24.9508106644704

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003494e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-6.79969959110062;-3.91129540483293;-4.44981053500876]
Robot Arm 1 end: [-4.29211504327366;-4.1421464460788;0.0777511708881612]
Robot Arm 1 togoal: 20.1580709815012
Mobile Platform 2 Pose: [1.48095351863922;5.51502385775795;8.81375482281994]
Robot Arm 2 end: [6.18176399102547;8.97465286911448;0.168296917996281]
Robot Arm 1 togoal: 24.9743216723239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001014e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-6.98993120171788;-3.80380192931662;-4.4492344904018]
Robot Arm 1 end: [-4.16039994254679;-3.97037084751532;0.190113873339595]
Robot Arm 1 togoal: 19.9363849790307
Mobile Platform 2 Pose: [1.42781139665113;5.5467340113894;8.81249504594022]
Robot Arm 2 end: [6.21470269148961;8.97292112142489;0.167465680091103]
Robot Arm 1 togoal: 24.9944068625039

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.617028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-7.18890611331857;-3.68998873517698;-4.45753254963566]
Robot Arm 1 end: [-3.93121704505348;-3.85431471703654;0.255362103958074]
Robot Arm 1 togoal: 19.6881356130788
Mobile Platform 2 Pose: [1.37933488983769;5.57539868686224;8.81082956851399]
Robot Arm 2 end: [6.24456725830173;8.97076813199443;0.167238277212259]
Robot Arm 1 togoal: 25.0121706321037

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.395688e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-7.38970373556361;-3.57065474527214;-4.4886329450029]
Robot Arm 1 end: [-3.64374489270301;-3.81616778389507;0.287617205023823]
Robot Arm 1 togoal: 19.4566055633773
Mobile Platform 2 Pose: [1.33414209967948;5.60193556950115;8.80889811655556]
Robot Arm 2 end: [6.27222335550232;8.96834646424109;0.167338939885885]
Robot Arm 1 togoal: 25.0283003574856
Step 141:
Mobile Platform 1 Pose: [-7.54221171663613;-3.48669989302936;-4.55295266179534]
Robot Arm 1 end: [-3.63534605207139;-3.87800183294394;0.303240774932969]
Robot Arm 1 togoal: 19.4937049932836
Mobile Platform 2 Pose: [1.29136949941346;5.62691328532366;8.80678177633983]
Robot Arm 2 end: [6.29823291413017;8.96574202814339;0.16763434905142]
Robot Arm 1 togoal: 25.0432299311907
Step 142:
Mobile Platform 1 Pose: [-7.72321363514801;-3.38469315912348;-4.5808812420879]
Robot Arm 1 end: [-3.57801975967778;-3.87999333794667;0.374956599957749]
Robot Arm 1 togoal: 19.4507133835863
Mobile Platform 2 Pose: [1.25044555772208;5.65070573956944;8.80453129144345]
Robot Arm 2 end: [6.32297458642864;8.96300752522736;0.168050344489206]
Robot Arm 1 togoal: 25.0572469860791
Step 143:
Mobile Platform 1 Pose: [-7.8703743575796;-3.30238076225067;-4.58951752750878]
Robot Arm 1 end: [-3.54270118769941;-3.87296233596465;0.4523150609818]
Robot Arm 1 togoal: 19.4166404646451
Mobile Platform 2 Pose: [1.21097329760328;5.67357126868985;8.802180389151]
Robot Arm 2 end: [6.34671396060069;8.96017747493006;0.1685415382287]
Robot Arm 1 togoal: 25.0705510725504
Step 144:
Mobile Platform 1 Pose: [-7.99842936585068;-3.23125969362526;-4.59211264983693]
Robot Arm 1 end: [-3.5247651358963;-3.87475242747384;0.52601865124632]
Robot Arm 1 togoal: 19.4014951689094
Mobile Platform 2 Pose: [1.17266621565669;5.69569548984982;8.79975255556215]
Robot Arm 2 end: [6.36964388670285;8.95727564102938;0.169078254676546]
Robot Arm 1 togoal: 25.0832859313311
Step 145:
Mobile Platform 1 Pose: [-8.11794904706542;-3.16534894475771;-4.59181412047567]
Robot Arm 1 end: [-3.51503729455924;-3.88757714654564;0.593252064863554]
Robot Arm 1 togoal: 19.4005605152463
Mobile Platform 2 Pose: [1.13531117996208;5.71721595929874;8.79726474594415]
Robot Arm 2 end: [6.39190847694501;8.95431901815893;0.169640189945509]
Robot Arm 1 togoal: 25.0955582981314
Step 146:
Mobile Platform 1 Pose: [-8.23387217893431;-3.10197716532966;-4.58947573321324]
Robot Arm 1 end: [-3.50798373608973;-3.91034354563652;0.651769440250973]
Robot Arm 1 togoal: 19.4092463516258
Mobile Platform 2 Pose: [1.09874577837296;5.73823715137429;8.79472954987519]
Robot Arm 2 end: [6.41361799163849;8.95132012273906;0.170213023527818]
Robot Arm 1 togoal: 25.1074494010399
Step 147:
Mobile Platform 1 Pose: [-8.3483666217692;-3.04012960587515;-4.58529058407964]
Robot Arm 1 end: [-3.49977222424905;-3.94022413203866;0.700240069855163]
Robot Arm 1 togoal: 19.4228562734993
Mobile Platform 2 Pose: [1.06284383696237;5.75883999226248;8.79215652386675]
Robot Arm 2 end: [6.43485845192662;8.94828838505426;0.170786472719737]
Robot Arm 1 togoal: 25.1190223063577
Step 148:
Mobile Platform 1 Pose: [-8.46180925098261;-2.97990517463137;-4.57920407482309]
Robot Arm 1 end: [-3.48739880373578;-3.97281881200098;0.738276846865404]
Robot Arm 1 togoal: 19.4361068009639
Mobile Platform 2 Pose: [1.02750581523387;5.77908816049584;8.78955304876202]
Robot Arm 2 end: [6.4556980419659;8.94523103630835;0.171353090847055]
Robot Arm 1 togoal: 25.1303267755683
Step 149:
Mobile Platform 1 Pose: [-8.57295433853472;-2.92243099740729;-4.57106500833098]
Robot Arm 1 end: [-3.4683404749371;-4.00175982271341;0.766206961430165]
Robot Arm 1 togoal: 19.4426126282672
Mobile Platform 2 Pose: [0.9926521997953;5.79903240413964;8.78692490314938]
Robot Arm 2 end: [6.47619153064011;8.94215369620694;0.171907497404843]
Robot Arm 1 togoal: 25.141402597819
Step 150:
Mobile Platform 1 Pose: [-8.6786491138213;-2.86999495183975;-4.56070838815513]
Robot Arm 1 end: [-3.44007368874039;-4.01791121597481;0.78486294774976]
Robot Arm 1 togoal: 19.4339731752779
Mobile Platform 2 Pose: [0.958218841974822;5.81871357826748;8.78427665911824]
Robot Arm 2 end: [6.49638339346973;8.93906077573734;0.172445855914163]
Robot Arm 1 togoal: 25.1522819324798
Step 151:
Mobile Platform 1 Pose: [-8.77350979453743;-2.82608596506771;-4.54801466769286]
Robot Arm 1 end: [-3.39884189958424;-4.00914581958689;0.795711275284025]
Robot Arm 1 togoal: 19.3987415720377
Mobile Platform 2 Pose: [0.924153584502294;5.83816483650728;8.78161196319515]
Robot Arm 2 end: [6.51631006742347;8.93595576150167;0.172965505518484]
Robot Arm 1 togoal: 25.1629909955699
Step 152:
Mobile Platform 1 Pose: [-8.85160140025436;-2.79436751142145;-4.53253050336368]
Robot Arm 1 end: [-3.33869511124934;-3.96501298976848;0.801554793167931]
Robot Arm 1 togoal: 19.3251125865001
Mobile Platform 2 Pose: [0.890413759010598;5.85741325202329;8.77893374095849]
Robot Arm 2 end: [6.53600162448839;8.93284142186363;0.173464694087801]
Robot Arm 1 togoal: 25.1735513074973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.677860e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-8.89652812808645;-2.78492420459608;-4.51076496525039]
Robot Arm 1 end: [-3.34350345099428;-3.93219896908027;0.751350720355628]
Robot Arm 1 togoal: 19.3066727447894
Mobile Platform 2 Pose: [0.856964299842585;5.876481036249;8.77624434894963]
Robot Arm 2 end: [6.55548302943174;8.92971995944541;0.173942379031834]
Robot Arm 1 togoal: 25.1839806288648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.763276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-8.95481152530678;-2.75362817372533;-4.50984318402976]
Robot Arm 1 end: [-3.33175740346649;-3.92864248154551;0.704424362215345]
Robot Arm 1 togoal: 19.2979178310983
Mobile Platform 2 Pose: [0.823776294303925;5.89538647327825;8.773545689746]
Robot Arm 2 end: [6.57477510294623;8.92659312654228;0.174398072670041]
Robot Arm 1 togoal: 25.1942936761802

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.909477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-9.03188141473019;-2.71246559840355;-4.50824678061]
Robot Arm 1 end: [-3.29683100359591;-3.91916954072219;0.660150358090359]
Robot Arm 1 togoal: 19.2688883319078
Mobile Platform 2 Pose: [0.790825854727014;5.91414464572806;8.7708393005995]
Robot Arm 2 end: [6.59389527169073;8.92346231390299;0.174831722629002]
Robot Arm 1 togoal: 25.2045026783816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.932649e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-9.1180439051048;-2.66714116457933;-4.50571337889938]
Robot Arm 1 end: [-3.25468118448568;-3.91199275142414;0.604011887303317]
Robot Arm 1 togoal: 19.2372290836509
Mobile Platform 2 Pose: [0.758093218143957;5.93276801160104;8.76812642260193]
Robot Arm 2 end: [6.61285816478717;8.92032861962499;0.175243619698109]
Robot Arm 1 togoal: 25.2146178181042

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-9.21545995021552;-2.61676745228545;-4.50200340507623]
Robot Arm 1 end: [-3.18287122646691;-3.89066293316902;0.54566931747265]
Robot Arm 1 togoal: 19.1752413397278
Mobile Platform 2 Pose: [0.725562038363089;5.9512668589184;8.7654080554762]
Robot Arm 2 end: [6.63167607981316;8.91719290463878;0.175634314793235]
Robot Arm 1 togoal: 25.2246475781541

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.947161e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-9.32399761919715;-2.56167511012143;-4.49692587560735]
Robot Arm 1 end: [-3.12548836970318;-3.89688659880489;0.487605288605509]
Robot Arm 1 togoal: 19.1434011270932
Mobile Platform 2 Pose: [0.693218802684847;5.96964967816696;8.7626850013859]
Robot Arm 2 end: [6.65035936927957;8.91405583635656;0.176004559616347]
Robot Arm 1 togoal: 25.2345990267653

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955042e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-9.4549076221228;-2.49771672049386;-4.48905699016174]
Robot Arm 1 end: [-3.03465246751534;-3.8927975881722;0.43505803813871]
Robot Arm 1 togoal: 19.0811755329528
Mobile Platform 2 Pose: [0.661052355339855;5.9879234661113;8.75995790033242]
Robot Arm 2 end: [6.66891675460608;8.91091792413499;0.176355256011007]
Robot Arm 1 togoal: 25.2444780495239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976976e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-9.61099972215471;-2.42710072474349;-4.47666457436656]
Robot Arm 1 end: [-2.89271142598677;-3.86261255589609;0.392723215702687]
Robot Arm 1 togoal: 18.9648897516079
Mobile Platform 2 Pose: [0.629053512373882;6.0060939720711;8.7572272590421]
Robot Arm 2 end: [6.68735557972424;8.9077795486895;0.176687410456869]
Robot Arm 1 togoal: 25.2542895375102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998027e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-9.75457505379016;-2.37354250698739;-4.45913649710757]
Robot Arm 1 end: [-2.76556652576394;-3.85443893753118;0.331190856305301]
Robot Arm 1 togoal: 18.8764618717572
Mobile Platform 2 Pose: [0.597214734650121;6.02416590565832;8.75449347462625]
Robot Arm 2 end: [6.7056820275344;8.90464098575674;0.17700210110154]
Robot Arm 1 togoal: 25.264037546633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002247e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-9.81087312584542;-2.36121371980535;-4.4464894013078]
Robot Arm 1 end: [-2.68342293542011;-3.88588875336446;0.259161992470924]
Robot Arm 1 togoal: 18.8488556413062
Mobile Platform 2 Pose: [0.565529858472212;6.04214310822429;8.75175685413309]
Robot Arm 2 end: [6.72390129783015;8.90150242593529;0.177300447976387]
Robot Arm 1 togoal: 25.273725429056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-9.89023956254602;-2.32115173364455;-4.43682788745141]
Robot Arm 1 end: [-2.57751128487797;-3.93469217590077;0.212176154377467]
Robot Arm 1 togoal: 18.8170788385768
Mobile Platform 2 Pose: [0.533993865933615;6.06002869837005;8.74901763076303]
Robot Arm 2 end: [6.74201776041198;8.89836399087894;0.177583591322938]
Robot Arm 1 togoal: 25.2833559455386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.941015e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-10.0081606230887;-2.24992881722148;-4.42275203264923]
Robot Arm 1 end: [-2.44236199881045;-4.00107796002362;0.175342089667986]
Robot Arm 1 togoal: 18.7790668395355
Mobile Platform 2 Pose: [0.502602697309339;6.07782519148047;8.74627597737707]
Robot Arm 2 end: [6.76003507885589;8.89522574726138;0.177852669045014]
Robot Arm 1 togoal: 25.2929313571421

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987891e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-10.1066264440858;-2.18476961133677;-4.4068357022177]
Robot Arm 1 end: [-2.27868103725899;-4.03154114118008;0.135986047995625]
Robot Arm 1 togoal: 18.6967488740321
Mobile Platform 2 Pose: [0.471353086020752;6.09553460389962;8.74353201777225]
Robot Arm 2 end: [6.77795632313707;8.89208771779062;0.178108805168642]
Robot Arm 1 togoal: 25.3024535070998

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999995e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-10.1317410066136;-2.16473920675615;-4.39716905591808]
Robot Arm 1 end: [-2.12011492649315;-4.06481976114783;0.0822435396953475]
Robot Arm 1 togoal: 18.6222007533225
Mobile Platform 2 Pose: [0.440242425284399;6.11315853779401;8.74078583608533]
Robot Arm 2 end: [6.79578405816781;8.88894989097542;0.178353094313839]
Robot Arm 1 togoal: 25.3119238862978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999284e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-10.1452104726255;-2.1493275355169;-4.3854485566282]
Robot Arm 1 end: [-1.97284435872067;-4.13792671683012;0.050988986418283]
Robot Arm 1 togoal: 18.5846954849591
Mobile Platform 2 Pose: [0.40926864830334;6.13069825644628;8.73803748467148]
Robot Arm 2 end: [6.8135204252491;8.88581222882154;0.178586593820274]
Robot Arm 1 togoal: 25.3213436923412

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-10.1405512329641;-2.13841197476302;-4.3666013058797]
Robot Arm 1 end: [-1.8489908825371;-4.23376409172017;0.0467974630872394]
Robot Arm 1 togoal: 18.5788023907094
Mobile Platform 2 Pose: [0.378430131899487;6.14815474550025;8.73528699073341]
Robot Arm 2 end: [6.8311672099135;8.88267467339494;0.178810317435043]
Robot Arm 1 togoal: 25.3307138788729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993377e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-10.1033384678231;-2.14093907575612;-4.34333194730065]
Robot Arm 1 end: [-1.76732831849078;-4.31848953232778;0.0680417582573671]
Robot Arm 1 togoal: 18.5903266753111
Mobile Platform 2 Pose: [0.347725613227029;6.16552876547821;8.73253436178744]
Robot Arm 2 end: [6.84872589853807;8.87953715230473;0.179025228874705]
Robot Arm 1 togoal: 25.3400351968118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952154e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-10.1399018310922;-2.10269868924315;-4.32252981270189]
Robot Arm 1 end: [-1.67810601760513;-4.40972070698423;0.0632655192731169]
Robot Arm 1 togoal: 18.6049241016462
Mobile Platform 2 Pose: [0.317154119142684;6.18282089634449;8.72977959026015]
Robot Arm 2 end: [6.86619772881874;8.87639958310489;0.179232239323644]
Robot Arm 1 togoal: 25.3493082306449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.603359e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-10.2255459861553;-2.01294468338452;-4.29751344730902]
Robot Arm 1 end: [-1.55647321162406;-4.43967194585516;0.0746903761935856]
Robot Arm 1 togoal: 18.5512266581705
Mobile Platform 2 Pose: [0.286714906983876;6.20003157483757;8.72702265728329]
Robot Arm 2 end: [6.88358373100311;8.8732618770499;0.179432204031631]
Robot Arm 1 togoal: 25.3585334282698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-10.6022319741952;-1.78158921062345;-4.29581866421358]
Robot Arm 1 end: [-1.35808619885293;-4.53854291005682;0.0619874902938782]
Robot Arm 1 togoal: 18.5068654707278
Mobile Platform 2 Pose: [0.256407414214883;6.21716112617771;8.7242635358604]
Robot Arm 2 end: [6.90088476518191;8.87012394200484;0.179625922385725]
Robot Arm 1 togoal: 25.3677111275077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-10.8058834463991;-1.65502089392455;-4.29769416554355]
Robot Arm 1 end: [-1.20788627233293;-4.46130605079996;0.0394036998051702]
Robot Arm 1 togoal: 18.3559585689553
Mobile Platform 2 Pose: [0.226231217241534;6.23420979005469;8.7215021934474]
Robot Arm 2 end: [6.91810155236249;8.86698568479773;0.179814138461615]
Robot Arm 1 togoal: 25.3768415781319

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975466e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-11.0344514933485;-1.50807082244779;-4.29547387344897]
Robot Arm 1 end: [-0.929480915312146;-4.37183472810491;0.126905833687887]
Robot Arm 1 togoal: 18.1092983043154
Mobile Platform 2 Pose: [0.196185994413222;6.25117774355012;8.71873859403479]
Robot Arm 2 end: [6.93523470207801;8.86384701296286;0.179997542876468]
Robot Arm 1 togoal: 25.3859249611462

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987481e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-11.2640230148499;-1.35280102844377;-4.28853399710563]
Robot Arm 1 end: [-0.712653957530308;-4.24505538543449;0.189718663996413]
Robot Arm 1 togoal: 17.8733004155214
Mobile Platform 2 Pose: [0.166271500847824;6.26806511741585;8.71597269985882]
Robot Arm 2 end: [6.95228473019954;8.86070783663783;0.180176769946392]
Robot Arm 1 togoal: 25.3949614019596

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981514e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-11.5281197207538;-1.14472344474743;-4.2621074716239]
Robot Arm 1 end: [-0.521442765114187;-4.0926683115434;0.169799239096027]
Robot Arm 1 togoal: 17.6388718883896
Mobile Platform 2 Pose: [0.136487540923071;6.28487201290504;8.71320447278889]
Robot Arm 2 end: [6.96925208329234;8.85756806922462;0.180352404860603]
Robot Arm 1 togoal: 25.4039509865319

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.935604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-11.6751508434105;-0.916630827167605;-4.1784969537477]
Robot Arm 1 end: [-0.27299870956243;-3.94297613398764;0.191628973978152]
Robot Arm 1 togoal: 17.3697528273849
Mobile Platform 2 Pose: [0.106833952629977;6.30159851194992;8.71043387536896]
Robot Arm 2 end: [6.98613714972696;8.85442762859816;0.180524981703636]
Robot Arm 1 togoal: 25.4128937696213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001483e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-11.5638803111308;-0.836018842160152;-4.0885504073154]
Robot Arm 1 end: [-0.109786566381435;-3.993098290314;0.264492836434194]
Robot Arm 1 togoal: 17.3087664041164
Mobile Platform 2 Pose: [0.0773105885500523;6.31824468863781;8.70766087165358]
Robot Arm 2 end: [7.00294027673303;8.85128643736493;0.18069498937339]
Robot Arm 1 togoal: 25.4217897859148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.881734e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-11.8030583529252;-0.597349066184682;-4.03075711107477]
Robot Arm 1 end: [0.0648955738261474;-3.79721798343553;0.18706077364434]
Robot Arm 1 togoal: 17.0542932395637
Mobile Platform 2 Pose: [0.0479173029365405;6.33481061717415;8.70488542782784]
Robot Arm 2 end: [7.01966178038031;8.84814442335526;0.180862872825291]
Robot Arm 1 togoal: 25.4306390568902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006383e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-11.6913248201854;-0.525384721415263;-3.9450161280457]
Robot Arm 1 end: [0.216662717690103;-3.84716522384407;0.264002585715835]
Robot Arm 1 togoal: 17.0011092466165
Mobile Platform 2 Pose: [0.018653947608953;6.35129637518784;8.70210751270316]
Robot Arm 2 end: [7.03630195190308;8.84500152011388;0.181029034279709]
Robot Arm 1 togoal: 25.4394415952569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.885961e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-11.8685559454904;-0.301187165792319;-3.87642567971026]
Robot Arm 1 end: [0.370164377970961;-3.64412308993007;0.186416767727627]
Robot Arm 1 togoal: 16.7534158130312
Mobile Platform 2 Pose: [-0.0104796435902354;6.36770205228029;8.69932709797705]
Robot Arm 2 end: [7.05286106982104;8.84185766653076;0.181193839200347]
Robot Arm 1 togoal: 25.448197412409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.737103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-11.7783136557692;-0.219584135195676;-3.79139922731466]
Robot Arm 1 end: [0.531464725669613;-3.69344013992788;0.265085100940551]
Robot Arm 1 togoal: 16.695531897508
Mobile Platform 2 Pose: [-0.0394836542929013;6.38402775051795;8.69654415849321]
Robot Arm 2 end: [7.06933940266318;8.83871280717291;0.181357616884786]
Robot Arm 1 togoal: 25.4569065204138

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.745046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-11.9177743286522;-0.0148612259254514;-3.72192985454665]
Robot Arm 1 end: [0.660418712712351;-3.49020295687253;0.186476212509583]
Robot Arm 1 togoal: 16.4619047554906
Mobile Platform 2 Pose: [-0.0683582922115625;6.40027358809389;8.69375867230977]
Robot Arm 2 end: [7.08573721488631;8.83556689204288;0.1815206638783]
Robot Arm 1 togoal: 25.4655689355779

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.316055e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-11.8346984376813;0.0683737158179503;-3.63814013606019]
Robot Arm 1 end: [0.822500438137057;-3.54030658179943;0.265707295993858]
Robot Arm 1 togoal: 16.4055527279591
Mobile Platform 2 Pose: [-0.0971037908417049;6.41643970047817;8.6909706207477]
Robot Arm 2 end: [7.1020547704109;8.83241987645249;0.181683246577728]
Robot Arm 1 togoal: 25.4741846805572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.878317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-11.9560611223274;0.262747349967367;-3.56878922305033]
Robot Arm 1 end: [0.938810567432826;-3.33724802833305;0.186546062743861]
Robot Arm 1 togoal: 16.1792492475022
Mobile Platform 2 Pose: [-0.125742940223144;6.43253750591188;8.68817998835401]
Robot Arm 2 end: [7.10888426960148;8.83019963169776;0.170805058772473]
Robot Arm 1 togoal: 25.4777033011126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.774470e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-11.8847596623748;0.355352791936524;-3.48131734064558]
Robot Arm 1 end: [1.11057620721151;-3.38390399471859;0.265955452082022]
Robot Arm 1 togoal: 16.1160131026534
Mobile Platform 2 Pose: [-0.161535191450063;6.45317313200508;8.68583029669562]
Robot Arm 2 end: [7.1181688315536;8.82848804816295;0.157614997726474]
Robot Arm 1 togoal: 25.4833766402691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015839e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-12.0246857784381;0.553929447472396;-3.41349102961411]
Robot Arm 1 end: [1.23294193696304;-3.18247227524909;0.186744669181265]
Robot Arm 1 togoal: 15.8877034103797
Mobile Platform 2 Pose: [-0.198198159403925;6.4753364832245;8.68409678023306]
Robot Arm 2 end: [7.12635306187082;8.8274930531176;0.142153771453993]
Robot Arm 1 togoal: 25.4889717483271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009988e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-11.9579849561823;0.645097739591072;-3.3284399927402]
Robot Arm 1 end: [1.40229801693028;-3.23180196286987;0.266425891612111]
Robot Arm 1 togoal: 15.8295615469199
Mobile Platform 2 Pose: [-0.24641479773913;6.50535013059518;8.68371457594327]
Robot Arm 2 end: [7.14459124685607;8.82744739307269;0.129578084859752]
Robot Arm 1 togoal: 25.5018791328527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007981e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-12.0887867958281;0.836832152249705;-3.26164410390074]
Robot Arm 1 end: [1.51693016978907;-3.03105379509341;0.186855829878589]
Robot Arm 1 togoal: 15.6061134630478
Mobile Platform 2 Pose: [-0.305241198596059;6.54278000024394;8.68504955792054]
Robot Arm 2 end: [7.17936486797963;8.82835176959226;0.126360720728246]
Robot Arm 1 togoal: 25.5261126367876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005856e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-12.0227692802693;0.926294577299453;-3.17754242965116]
Robot Arm 1 end: [1.68388238846047;-3.08142307509932;0.266545024065592]
Robot Arm 1 togoal: 15.5516911969517
Mobile Platform 2 Pose: [-0.364281154778659;6.58054504901477;8.68664395729199]
Robot Arm 2 end: [7.21620784738461;8.82938091439876;0.125081492854142]
Robot Arm 1 togoal: 25.5517511482572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004005e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-12.1505986399868;1.11549875936394;-3.11089274066549]
Robot Arm 1 end: [1.79588132515399;-2.88062649918082;0.186919692819878]
Robot Arm 1 togoal: 15.3296403254728
Mobile Platform 2 Pose: [-0.421834360724905;6.61756085286524;8.68827492901551]
Robot Arm 2 end: [7.25069761110258;8.83054096632572;0.122552764649839]
Robot Arm 1 togoal: 25.5759945226814

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002920e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-12.0833354557333;1.20339680308593;-3.02665480542043]
Robot Arm 1 end: [1.96066976860952;-2.93078696530406;0.266512542763731]
Robot Arm 1 togoal: 15.2777557544225
Mobile Platform 2 Pose: [-0.480248768818517;6.65556645478932;8.6904693390281]
Robot Arm 2 end: [7.28704065906711;8.8321717539779;0.121200572559549]
Robot Arm 1 togoal: 25.6017949791929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002124e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-12.2112214414037;1.39237723151685;-2.95943173639163]
Robot Arm 1 end: [2.07275848565919;-2.7293795062852;0.186984646879958]
Robot Arm 1 togoal: 15.0551604974534
Mobile Platform 2 Pose: [-0.539852773753379;6.69482603791734;8.69333188918041]
Robot Arm 2 end: [7.32547736614353;8.83438075097401;0.120933373420708]
Robot Arm 1 togoal: 25.6294020764487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001613e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-12.1393572171053;1.47672457572755;-2.87511743602653]
Robot Arm 1 end: [2.23312373238679;-2.77921190400086;0.266376653349854]
Robot Arm 1 togoal: 15.0068885377926
Mobile Platform 2 Pose: [-0.601513193387085;6.73587437037786;8.69696911957374]
Robot Arm 2 end: [7.3676308560923;8.83720597434939;0.122619674696522]
Robot Arm 1 togoal: 25.6598986359602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000978e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-12.2653128315999;1.66587693319509;-2.80638464185612]
Robot Arm 1 end: [2.34560006253883;-2.57635407046475;0.187038056183565]
Robot Arm 1 togoal: 14.7828946615963
Mobile Platform 2 Pose: [-0.663749090592737;6.77764632841802;8.70101218159575]
Robot Arm 2 end: [7.40835336877917;8.84056892708638;0.1222743948893]
Robot Arm 1 togoal: 25.6899637568248

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000903e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-12.1858219497227;1.74579341385435;-2.72117623119757]
Robot Arm 1 end: [2.50050520207634;-2.62484259310287;0.266111585277799]
Robot Arm 1 togoal: 14.7378362610701
Mobile Platform 2 Pose: [-0.728433745595332;6.82173563246985;8.70623825602429]
Robot Arm 2 end: [7.45356035754493;8.84491205627877;0.124306029585272]
Robot Arm 1 togoal: 25.7236901242869

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000804e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-12.311771671339;1.93728772668723;-2.6498751721392]
Robot Arm 1 end: [2.61589462553304;-2.41966309368711;0.187120549990841]
Robot Arm 1 togoal: 14.5104196331148
Mobile Platform 2 Pose: [-0.795316386925457;6.86782385839735;8.71237614992231]
Robot Arm 2 end: [7.49972661500195;8.85021080815465;0.125336685154782]
Robot Arm 1 togoal: 25.758857800766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000744e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-12.22122754923;2.01096471786126;-2.56354345442541]
Robot Arm 1 end: [2.7631322578132;-2.46644204084281;0.265759377197036]
Robot Arm 1 togoal: 14.4692799024945
Mobile Platform 2 Pose: [-0.86534059218112;6.91681372546364;8.7200024244221]
Robot Arm 2 end: [7.55039169921106;8.85682289691195;0.128125487785471]
Robot Arm 1 togoal: 25.7979867037724

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000661e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-12.3482592854336;2.20657600552646;-2.48862950033711]
Robot Arm 1 end: [2.88351316281877;-2.2580375407824;0.187253452707327]
Robot Arm 1 togoal: 14.2366691718049
Mobile Platform 2 Pose: [-0.938560024185505;6.96856450862428;8.72884700984989]
Robot Arm 2 end: [7.60205580239131;8.86475867316615;0.129039668901049]
Robot Arm 1 togoal: 25.838905585002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000642e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-12.2439382498063;2.27267553184625;-2.40086102553999]
Robot Arm 1 end: [3.02132350901666;-2.30278140074308;0.26536842825769]
Robot Arm 1 togoal: 14.1998931828458
Mobile Platform 2 Pose: [-1.01644014270107;7.02443176828528;8.73987560147856]
Robot Arm 2 end: [7.6625257730222;8.87442614065759;0.134846148870113]
Robot Arm 1 togoal: 25.8868731468444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003018e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-12.3762949739661;2.47518118420596;-2.32160102463008]
Robot Arm 1 end: [3.15021721196457;-2.09047920265747;0.187476154555311]
Robot Arm 1 togoal: 13.9598530593817
Mobile Platform 2 Pose: [-1.12576733740496;7.09606080216878;8.75049385399814]
Robot Arm 2 end: [7.76688495659079;8.88065643774392;0.170171883819363]
Robot Arm 1 togoal: 25.9608575586462

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001758e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-12.2561012849255;2.53177730434422;-2.23295577895257]
Robot Arm 1 end: [3.2760468671497;-2.13386239090058;0.265049670703495]
Robot Arm 1 togoal: 13.9290909065789
Mobile Platform 2 Pose: [-1.17596044544328;7.12601636854287;8.749580904557]
Robot Arm 2 end: [7.7864665047284;8.88028815353784;0.158191653727284]
Robot Arm 1 togoal: 25.9746206531129

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002370e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-12.4023782887888;2.74570003519077;-2.14903886042095]
Robot Arm 1 end: [3.41871262214311;-1.91734320788717;0.187802605318948]
Robot Arm 1 togoal: 13.6789556610765
Mobile Platform 2 Pose: [-1.23810997515361;7.16451159021252;8.75047438396299]
Robot Arm 2 end: [7.81998928663882;8.88113662707221;0.151181301725778]
Robot Arm 1 togoal: 25.9985719179387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002758e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-12.2683584949661;2.79294850182467;-2.06016877384729]
Robot Arm 1 end: [3.5317786236662;-1.96138980381198;0.264745731663738]
Robot Arm 1 togoal: 13.6561674198405
Mobile Platform 2 Pose: [-1.30110522184927;7.20407364809605;8.7519317505245]
Robot Arm 2 end: [7.85949309007481;8.88206903888839;0.149311982113477]
Robot Arm 1 togoal: 26.02643949222

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002297e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002690e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-12.452053993702;3.02433865370088;-1.9759217235412]
Robot Arm 1 end: [3.69779884409408;-1.74523151982381;0.187692519773292]
Robot Arm 1 togoal: 13.3957422974837
Mobile Platform 2 Pose: [-1.36235547098624;7.24172882362475;8.75262724841653]
Robot Arm 2 end: [7.89798554333473;8.88214742273713;0.14858831388035]
Robot Arm 1 togoal: 26.0529632026684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002248e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-12.3295147263846;3.07921997591232;-1.88591492632572]
Robot Arm 1 end: [3.8201610540131;-1.7882395304067;0.264734395172825]
Robot Arm 1 togoal: 13.3690390041488
Mobile Platform 2 Pose: [-1.41769631717529;7.27539223382014;8.75258246676453]
Robot Arm 2 end: [7.93133960705272;8.88161069948632;0.147157604344194]
Robot Arm 1 togoal: 26.0755748860599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001708e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001791e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-12.5156146773017;3.30674851132544;-1.80497209901382]
Robot Arm 1 end: [3.9822527482461;-1.57506198294384;0.187694283733476]
Robot Arm 1 togoal: 13.1138493977936
Mobile Platform 2 Pose: [-1.47046276693213;7.30733391364284;8.7522495688137]
Robot Arm 2 end: [7.96380356035353;8.88073338238246;0.146589401610601]
Robot Arm 1 togoal: 26.0973051464044

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000291e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-12.3959240032796;3.36083063094456;-1.71677261562336]
Robot Arm 1 end: [4.10389078276553;-1.61949565818217;0.264882866491455]
Robot Arm 1 togoal: 13.0902487946244
Mobile Platform 2 Pose: [-1.51964383652859;7.33689323472444;8.75152404759566]
Robot Arm 2 end: [7.96662144034678;8.83840668336927;0.109689335939813]
Robot Arm 1 togoal: 26.0706127687715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000728e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001681e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-12.5621887613095;3.57704339932713;-1.63629824809605]
Robot Arm 1 end: [4.25286214997621;-1.40654298787767;0.18785820995557]
Robot Arm 1 togoal: 12.841965715335
Mobile Platform 2 Pose: [-1.65520298139717;7.42773667060967;8.76144075483011]
Robot Arm 2 end: [8.07998710207891;8.83896553987046;0.122004880541884]
Robot Arm 1 togoal: 26.1486088266285

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993590e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000410e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-12.4365879906971;3.62267905229197;-1.55203952524794]
Robot Arm 1 end: [4.36941044404998;-1.51538067536797;0.264884576151573]
Robot Arm 1 togoal: 12.8796489394797
Mobile Platform 2 Pose: [-1.71572943677294;7.46503712263504;8.75993071528609]
Robot Arm 2 end: [8.16004155399694;8.82197452504497;0.163805746991464]
Robot Arm 1 togoal: 26.1896126451831

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000654e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996014e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-12.6368274862208;3.84344925277603;-1.48879297741765]
Robot Arm 1 end: [4.53979820338577;-1.31455081207183;0.188565716428999]
Robot Arm 1 togoal: 12.6336156684751
Mobile Platform 2 Pose: [-1.74340175306284;7.4809762704741;8.75657389387369]
Robot Arm 2 end: [8.20378440971438;8.75089082581272;0.204025467443919]
Robot Arm 1 togoal: 26.1669275138735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997974e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987597e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-12.5336016237223;3.89481821400453;-1.41753430418273]
Robot Arm 1 end: [4.64347351098329;-1.37887988974907;0.265521060243082]
Robot Arm 1 togoal: 12.6391261937306
Mobile Platform 2 Pose: [-1.79371789264897;7.51315128137522;8.75305074619012]
Robot Arm 2 end: [8.23199516980406;8.77730022547518;0.200110847565883]
Robot Arm 1 togoal: 26.2056749537654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996315e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997154e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-12.676100752298;4.10028862225866;-1.34492226300489]
Robot Arm 1 end: [4.79084806514926;-1.17053070293971;0.188594087848897]
Robot Arm 1 togoal: 12.3972305609121
Mobile Platform 2 Pose: [-1.84067833572265;7.5428863392979;8.74999998155221]
Robot Arm 2 end: [8.22236579417117;8.87894523729281;0.171886455038929]
Robot Arm 1 togoal: 26.2733571468604

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000366e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000936e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-12.571133419268;4.1664092097585;-1.25865974645269]
Robot Arm 1 end: [4.9117623946345;-1.21969639820343;0.265454695091733]
Robot Arm 1 togoal: 12.3833817355839
Mobile Platform 2 Pose: [-1.86646000897079;7.55733781753658;8.74745616780661]
Robot Arm 2 end: [8.21964687000165;8.88062574533541;0.158193678857399]
Robot Arm 1 togoal: 26.2733858052579

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001643e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-12.7572351277655;4.38962832388848;-1.18438439211124]
Robot Arm 1 end: [5.08509251026816;-1.00923767144057;0.189419018495046]
Robot Arm 1 togoal: 12.129828426206
Mobile Platform 2 Pose: [-1.92220439906895;7.59143131809852;8.74822280873692]
Robot Arm 2 end: [8.24159960818919;8.8815123436183;0.147210214795203]
Robot Arm 1 togoal: 26.2898224227862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000719e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002482e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-12.629517290378;4.42590634250667;-1.10931858477592]
Robot Arm 1 end: [5.16920147729198;-1.09387062648056;0.266303419660033]
Robot Arm 1 togoal: 12.1649021102844
Mobile Platform 2 Pose: [-2.04228559403731;7.66407627681025;8.75532713987418]
Robot Arm 2 end: [8.31402038895867;8.88283446583048;0.147168451329219]
Robot Arm 1 togoal: 26.3410742642602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996059e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002773e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-12.7722476914298;4.64250660668956;-1.02656426239727]
Robot Arm 1 end: [5.33535434877686;-0.874831519787241;0.190285548053403]
Robot Arm 1 togoal: 11.9076311912929
Mobile Platform 2 Pose: [-2.13193296390061;7.72249361030082;8.76296834416418]
Robot Arm 2 end: [8.3777787500752;8.88454523431497;0.151416287548595]
Robot Arm 1 togoal: 26.3864465216333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001362e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002406e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-12.6262163891906;4.68375601026155;-0.937761911078685]
Robot Arm 1 end: [5.41780454287398;-0.919340247550031;0.267462698338901]
Robot Arm 1 togoal: 11.9079747403044
Mobile Platform 2 Pose: [-2.20593745368204;7.76925834850726;8.76622330735092]
Robot Arm 2 end: [8.42157591294384;8.8862422041178;0.146946530572808]
Robot Arm 1 togoal: 26.4184145369437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993334e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002361e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-12.7695443569501;4.91248546124248;-0.836831020020775]
Robot Arm 1 end: [5.60018905297617;-0.684117087888114;0.191243655411779]
Robot Arm 1 togoal: 11.6308618018282
Mobile Platform 2 Pose: [-2.28073161863968;7.81661986828189;8.76963599346742]
Robot Arm 2 end: [8.46866423783449;8.88890942839523;0.145959852670003]
Robot Arm 1 togoal: 26.4532265240132

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001730e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-12.603147395012;4.95439507945388;-0.728959410594356]
Robot Arm 1 end: [5.68237285819201;-0.707978921273865;0.268949685726775]
Robot Arm 1 togoal: 11.6133693088438
Mobile Platform 2 Pose: [-2.35340450427493;7.86251732306424;8.77273389378539]
Robot Arm 2 end: [8.51523600234296;8.89118562210535;0.145908026620944]
Robot Arm 1 togoal: 26.4873882511385

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996607e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002711e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-12.7120050137204;5.17275314955899;-0.607222996390928]
Robot Arm 1 end: [5.85294439394651;-0.453875610045751;0.192589331944076]
Robot Arm 1 togoal: 11.3245865431376
Mobile Platform 2 Pose: [-2.42201002203297;7.9055751373432;8.77515541955851]
Robot Arm 2 end: [8.55751041258894;8.89296757722199;0.144595463942686]
Robot Arm 1 togoal: 26.5182937994545

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001910e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003081e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-12.5244571234754;5.21583048226064;-0.472854813724385]
Robot Arm 1 end: [5.93794320840178;-0.450090531549621;0.271503208923061]
Robot Arm 1 togoal: 11.2812206665176
Mobile Platform 2 Pose: [-2.49031545039067;7.94845654529375;8.77759564751688]
Robot Arm 2 end: [8.60098136297163;8.89462252120367;0.14465440610444]
Robot Arm 1 togoal: 26.5499076284312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000149e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003235e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-12.5992410733312;5.42045401613167;-0.326058846476876]
Robot Arm 1 end: [6.08676137259352;-0.188053717973113;0.180397529437389]
Robot Arm 1 togoal: 10.995776011697
Mobile Platform 2 Pose: [-2.5556593717613;7.98928600065881;8.77957006503678]
Robot Arm 2 end: [8.64153404464128;8.89593256471466;0.143918768626636]
Robot Arm 1 togoal: 26.5793041998011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000717e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003711e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-12.4223760256192;5.47330836576953;-0.166313535291201]
Robot Arm 1 end: [6.21013872101305;-0.174265082023084;0.316706069799966]
Robot Arm 1 togoal: 10.9237833347039
Mobile Platform 2 Pose: [-2.61998960714993;8.02943363940129;8.78142672416683]
Robot Arm 2 end: [8.68217935595877;8.89705536105844;0.143907505506157]
Robot Arm 1 togoal: 26.6086247398379

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004026e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-12.5954470603481;5.63277403432394;-0.107456532078243]
Robot Arm 1 end: [6.30958459537909;0.0300195919101684;0.182353323615313]
Robot Arm 1 togoal: 10.7150198376183
Mobile Platform 2 Pose: [-2.68171458749871;8.06778845418816;8.78289564909551]
Robot Arm 2 end: [8.7208150918074;8.89784749538612;0.14364020419298]
Robot Arm 1 togoal: 26.6363412965749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.902870e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004440e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-12.3671131379646;5.62754167268441;0.0175518173989828]
Robot Arm 1 end: [6.34619223670689;0.0286007488307321;0.260642473835179]
Robot Arm 1 togoal: 10.6942795592354
Mobile Platform 2 Pose: [-2.74391778364002;8.10643273499001;8.78425499030332]
Robot Arm 2 end: [8.76056766239731;8.89844922116082;0.144005923172202]
Robot Arm 1 togoal: 26.6647031610667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.922018e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004262e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-12.1160524561529;5.62865719527478;0.163025773904575]
Robot Arm 1 end: [6.3928164400669;0.038668231270761;0.467890963547507]
Robot Arm 1 togoal: 10.6465460354241
Mobile Platform 2 Pose: [-2.80721277723088;8.14575485018141;8.78540114932071]
Robot Arm 2 end: [8.79874970519311;8.89904695558067;0.142466071992345]
Robot Arm 1 togoal: 26.69208350988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004656e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-12.36770337679;5.82937923666073;0.216154084610101]
Robot Arm 1 end: [6.55118596659173;0.222049714529248;0.257688419648732]
Robot Arm 1 togoal: 10.4450268631163
Mobile Platform 2 Pose: [-2.86299931771183;8.18006724682721;8.78607285602946]
Robot Arm 2 end: [8.83241663158302;8.89918446953984;0.14157389835057]
Robot Arm 1 togoal: 26.7159485358303

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005652e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-12.4563687927213;6.02699563097132;0.369333153572926]
Robot Arm 1 end: [6.73288342921596;0.493014839522021;0.211134480578689]
Robot Arm 1 togoal: 10.1376804648602
Mobile Platform 2 Pose: [-2.91912744451118;8.21463154658274;8.78683035972086]
Robot Arm 2 end: [8.86827518731372;8.89920731319396;0.142335298147914]
Robot Arm 1 togoal: 26.7412146689603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006878e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-12.5337078337664;6.17486490163008;0.491956285875931]
Robot Arm 1 end: [6.8994212922857;0.755827608802422;0.279062652693734]
Robot Arm 1 togoal: 9.8290752309434
Mobile Platform 2 Pose: [-2.97657647552774;8.25003151108014;8.78745689467746]
Robot Arm 2 end: [8.90538967890332;8.8990602885803;0.143228706055533]
Robot Arm 1 togoal: 26.7672653149513

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-12.5693651228176;6.27304491023209;0.59473559381976]
Robot Arm 1 end: [6.94813215723703;0.990682357724491;0.294168627447615]
Robot Arm 1 togoal: 9.59097580675638
Mobile Platform 2 Pose: [-3.03374816294094;8.28520709664242;8.78777178372864]
Robot Arm 2 end: [8.94242727510324;8.89860336446298;0.144057949408104]
Robot Arm 1 togoal: 26.793072418268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010394e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-12.3948254075911;6.24157969083553;0.680337711392184]
Robot Arm 1 end: [6.98575537203892;0.954851966929617;0.385978764599326]
Robot Arm 1 togoal: 9.60149190606125
Mobile Platform 2 Pose: [-3.08971548770579;8.31956825232549;8.78773021942418]
Robot Arm 2 end: [8.97418587967713;8.89825324714943;0.141281998902039]
Robot Arm 1 togoal: 26.8154307694746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009531e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-12.4152769706267;6.36445790888976;0.814799511764469]
Robot Arm 1 end: [7.03413053589003;1.18207052724289;0.305194972570172]
Robot Arm 1 togoal: 9.38245403555925
Mobile Platform 2 Pose: [-3.13475959680546;8.34664051714235;8.78707403510283]
Robot Arm 2 end: [9.00061744098721;8.89718951584188;0.140359277758235]
Robot Arm 1 togoal: 26.8334381610017

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.930152e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-12.172735248406;6.31290111000235;0.915028529090869]
Robot Arm 1 end: [7.03993057374388;1.15555003733209;0.387238251368234]
Robot Arm 1 togoal: 9.3953031804755
Mobile Platform 2 Pose: [-3.18370467284041;8.37631801758333;8.78673872570534]
Robot Arm 2 end: [9.03224082676359;8.89613375008332;0.141559433752923]
Robot Arm 1 togoal: 26.8550352422925

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012558e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-12.1733852738136;6.45527214150121;1.08548592891722]
Robot Arm 1 end: [7.15661411511951;1.30833218029979;0.310016940861454]
Robot Arm 1 togoal: 9.22477169082631
Mobile Platform 2 Pose: [-3.23210014608576;8.4056115335366;8.7861604024382]
Robot Arm 2 end: [9.06202974906977;8.89499902252007;0.14147599750686]
Robot Arm 1 togoal: 26.8753614429445

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.002712e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.718888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-11.9128410367192;6.43544625252709;1.23030947034942]
Robot Arm 1 end: [7.19773911408162;1.28345133897479;0.520685509016553]
Robot Arm 1 togoal: 9.21040258151835
Mobile Platform 2 Pose: [-3.28211303514465;8.43599505875444;8.78562040673265]
Robot Arm 2 end: [9.09394329663671;8.89377341390982;0.142230384139614]
Robot Arm 1 togoal: 26.8971067233069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.149413e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-12.1161066395961;6.65565583982205;1.34928833578401]
Robot Arm 1 end: [7.36688297611446;1.43330568737749;0.287465000353046]
Robot Arm 1 togoal: 9.04672124599563
Mobile Platform 2 Pose: [-3.33192971172428;8.46626945122893;8.78493083008011]
Robot Arm 2 end: [9.1253493617123;8.89244176612977;0.142637128468824]
Robot Arm 1 togoal: 26.9184551618359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.215000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.320299e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-11.8866890351892;6.64074386544774;1.48166991488218]
Robot Arm 1 end: [7.4060512786571;1.42891083171264;0.448435606946353]
Robot Arm 1 togoal: 9.01900958403615
Mobile Platform 2 Pose: [-3.38210009784365;8.49681846637693;8.78417172582882]
Robot Arm 2 end: [9.15773903952091;8.89095442654251;0.143639548870079]
Robot Arm 1 togoal: 26.9403831833259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.366123e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-12.003789079995;6.76068847879083;1.53952066207305]
Robot Arm 1 end: [7.51499276053774;1.63058053964526;0.366772859606949]
Robot Arm 1 togoal: 8.80651369017072
Mobile Platform 2 Pose: [-3.43147934378083;8.52689420732378;8.78325337291482]
Robot Arm 2 end: [9.18421087647416;8.88990184451266;0.140309684930176]
Robot Arm 1 togoal: 26.9586469318191
Step 240:
Mobile Platform 1 Pose: [-11.8171239034595;6.78119667994848;1.67125299266746]
Robot Arm 1 end: [7.56642397877463;1.62322351750059;0.571873785429355]
Robot Arm 1 togoal: 8.77459497982804
Mobile Platform 2 Pose: [-3.46407229520698;8.54573207343204;8.78131920821943]
Robot Arm 2 end: [9.20868554660306;8.88701048356264;0.144483821587145]
Robot Arm 1 togoal: 26.9738310631604
Step 241:
Mobile Platform 1 Pose: [-12.0632577686603;6.98598683191711;1.72915030373064]
Robot Arm 1 end: [7.73727823327993;1.81169299353012;0.356189805366418]
Robot Arm 1 togoal: 8.57467574986453
Mobile Platform 2 Pose: [-3.48827416952569;8.55911597147344;8.77866368471613]
Robot Arm 2 end: [9.22539180217737;8.88371845189182;0.147000257167766]
Robot Arm 1 togoal: 26.9832983123052
Step 242:
Mobile Platform 1 Pose: [-12.1044019259153;7.15124398224532;1.87691758005762]
Robot Arm 1 end: [7.86508531305719;1.97939892242996;0.280329270040962]
Robot Arm 1 togoal: 8.39208950299143
Mobile Platform 2 Pose: [-3.50834248500951;8.56985232635157;8.77571253237504]
Robot Arm 2 end: [9.23862453353556;8.88024723915801;0.149055090342855]
Robot Arm 1 togoal: 26.9901953047625
Step 243:
Mobile Platform 1 Pose: [-11.8020591499533;7.11132776527177;2.00973952993683]
Robot Arm 1 end: [7.83877407303236;1.98260420099588;0.370878217043228]
Robot Arm 1 togoal: 8.38285828467341
Mobile Platform 2 Pose: [-3.52582878078107;8.57894944582854;8.77259733072886]
Robot Arm 2 end: [9.2497924630599;8.87667232223746;0.150927865808734]
Robot Arm 1 togoal: 26.9955619590497

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.085971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-11.4104174851037;6.99119319373637;2.11411990711319]
Robot Arm 1 end: [7.75393169539419;2.10529775225974;0.594833866390614]
Robot Arm 1 togoal: 8.2600805151327
Mobile Platform 2 Pose: [-3.54153205075633;8.58692212825914;8.76937939907567]
Robot Arm 2 end: [9.25958158147575;8.87303048592149;0.152724307007375]
Robot Arm 1 togoal: 26.9999063107278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.878907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-11.106819854947;6.90203737279281;2.20864198735653]
Robot Arm 1 end: [7.64067611517628;2.37379443151755;0.707939653083839]
Robot Arm 1 togoal: 8.024119122619
Mobile Platform 2 Pose: [-3.55593058443453;8.59407663023939;8.76609296824504]
Robot Arm 2 end: [9.26838743823485;8.86934284023311;0.154493532678272]
Robot Arm 1 togoal: 27.0035218743299

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.220623e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-10.7963038854968;6.80719923457753;2.3490875050182]
Robot Arm 1 end: [7.48345117543633;2.62545095832449;0.930599692350306]
Robot Arm 1 togoal: 7.8144458586494
Mobile Platform 2 Pose: [-3.5693370875751;8.6006122239874;8.76275929366631]
Robot Arm 2 end: [9.27646154815393;8.86562275184837;0.156261214792733]
Robot Arm 1 togoal: 27.0065955570924

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-10.7607146994231;6.86262734448077;2.54569820881776]
Robot Arm 1 end: [7.52444410582958;2.85491369040113;0.954097087539397]
Robot Arm 1 togoal: 7.58300825984882
Mobile Platform 2 Pose: [-3.58196832640982;8.60666651527139;8.75939254175857]
Robot Arm 2 end: [9.28397445855329;8.8618792636802;0.158041819122662]
Robot Arm 1 togoal: 27.0092542525447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.180367e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-10.6287335593984;6.8511250411521;2.70021236545618]
Robot Arm 1 end: [7.51761065657485;3.12778458780874;0.860339745036224]
Robot Arm 1 togoal: 7.33662548426718
Mobile Platform 2 Pose: [-3.59398095296039;8.61233863852371;8.75600264159753]
Robot Arm 2 end: [9.29104718319924;8.85811879085285;0.159843914762184]
Robot Arm 1 togoal: 27.0115880992347
Step 249:
Mobile Platform 1 Pose: [-10.846910913017;7.03752511696275;2.80440632387417]
Robot Arm 1 end: [7.76150551049346;3.13035284130796;0.689443618096446]
Robot Arm 1 togoal: 7.27285333832337
Mobile Platform 2 Pose: [-3.60549174769731;8.61770230568909;8.75259681653545]
Robot Arm 2 end: [9.29776852081371;8.85434604703449;0.161672750595315]
Robot Arm 1 togoal: 27.0136633079486
Step 250:
Mobile Platform 1 Pose: [-10.8556284747719;7.16857305781584;3.01730638774961]
Robot Arm 1 end: [7.84434903097134;3.39277121786453;0.595758077721725]
Robot Arm 1 togoal: 7.01130343762804
Mobile Platform 2 Pose: [-3.6165898767027;8.62281367826547;8.7491804737139]
Robot Arm 2 end: [9.30420532279611;8.85056459053047;0.163531606776865]
Robot Arm 1 togoal: 27.0155297739102
Step 251:
Mobile Platform 1 Pose: [-11.100248101135;7.35698712241573;3.17077295931405]
Robot Arm 1 end: [8.06717259344163;3.43664913622942;0.706827271337787]
Robot Arm 1 togoal: 6.89030286537526
Mobile Platform 2 Pose: [-3.62734472184004;8.62771637837867;8.74575775132118]
Robot Arm 2 end: [9.31040893885184;8.8467771632821;0.165422554695489]
Robot Arm 1 togoal: 27.0172258575583
Step 252:
Mobile Platform 1 Pose: [-10.7588388336922;7.21350609538517;3.36221984199791]
Robot Arm 1 end: [7.87919321699067;3.73200918871141;0.591327683765859]
Robot Arm 1 togoal: 6.68205214263019
Mobile Platform 2 Pose: [-3.63781109783693;8.63244481808505;8.74233187263749]
Robot Arm 2 end: [9.31641944612688;8.84298591264883;0.167346902133089]
Robot Arm 1 togoal: 27.0187815238281
Step 253:
Mobile Platform 1 Pose: [-10.6121383221449;7.20078707441197;3.61189608542435]
Robot Arm 1 end: [7.81268123113611;4.02951960295567;0.445604720193769]
Robot Arm 1 togoal: 6.44883513325813
Mobile Platform 2 Pose: [-3.64803285500845;8.63702649074462;8.73890538291542]
Robot Arm 2 end: [9.32226852991994;8.83919254028194;0.169305468412871]
Robot Arm 1 togoal: 27.0202204805748
Step 254:
Mobile Platform 1 Pose: [-10.8187500982209;7.37241357422597;3.79367712008309]
Robot Arm 1 end: [8.08928187524013;4.05492733966759;0.646718715445597]
Robot Arm 1 togoal: 6.30548177805946
Mobile Platform 2 Pose: [-3.65804543437156;8.64148359378152;8.73548031501014]
Robot Arm 2 end: [9.32798150300819;8.8353984079021;0.171298754626598]
Robot Arm 1 togoal: 27.0215616798288
Step 255:
Mobile Platform 1 Pose: [-10.4763711870889;7.23994791977944;3.94486406251052]
Robot Arm 1 end: [7.8563598673203;4.37138227847618;0.422237409388876]
Robot Arm 1 togoal: 6.12240228211226
Mobile Platform 2 Pose: [-3.66787773038695;8.64583420727842;8.73205830629113]
Robot Arm 2 end: [9.33357876124911;8.83160461153862;0.17332705768879]
Robot Arm 1 togoal: 27.0228203991031
Step 256:
Mobile Platform 1 Pose: [-10.6595005618088;7.41603804875084;4.09848259169892]
Robot Arm 1 end: [8.10195835271896;4.3639879393061;0.434520657955642]
Robot Arm 1 togoal: 6.04546370461271
Mobile Platform 2 Pose: [-3.6775534725311;8.65009316744095;8.72864068481395]
Robot Arm 2 end: [9.33907685533337;8.82781203678595;0.175390546637515]
Robot Arm 1 togoal: 27.024009038446
Step 257:
Mobile Platform 1 Pose: [-10.6377571687158;7.52675982871924;4.30875500849401]
Robot Arm 1 end: [8.25973635740924;4.46974733230361;0.520355784640397]
Robot Arm 1 togoal: 5.88332397204326
Mobile Platform 2 Pose: [-3.68709226868842;8.65427272502084;8.72522853345363]
Robot Arm 2 end: [9.34448929015837;8.82402140061311;0.177489310941994]
Robot Arm 1 togoal: 27.0251377158268
Step 258:
Mobile Platform 1 Pose: [-10.4362151079731;7.53417012131314;4.49476205137544]
Robot Arm 1 end: [8.25635152502227;4.77120868096714;0.64270183522229]
Robot Arm 1 togoal: 5.58139555385008
Mobile Platform 2 Pose: [-3.69651040724304;8.65838305028708;8.72182273830205]
Robot Arm 2 end: [9.34982713632757;8.82023328253042;0.179623397041093]
Robot Arm 1 togoal: 27.0262147221099
Step 259:
Mobile Platform 1 Pose: [-10.6930318389554;7.73937788251992;4.63692352548134]
Robot Arm 1 end: [8.47482448345452;4.7984917822643;0.750080516098131]
Robot Arm 1 togoal: 5.47504930992419
Mobile Platform 2 Pose: [-3.70582148172182;8.66243262514895;8.71842402550478]
Robot Arm 2 end: [9.35509950147217;8.81644814828472;0.181792833989655]
Robot Arm 1 togoal: 27.0272468716751
Step 260:
Mobile Platform 1 Pose: [-10.4663290036949;7.68418203686742;4.8807542453402]
Robot Arm 1 end: [8.41552083294743;5.15232187579303;0.657185519932457]
Robot Arm 1 togoal: 5.1726910677821
Mobile Platform 2 Pose: [-3.71503688028518;8.66642855100024;8.71503299102057]
Robot Arm 2 end: [9.36031389780071;8.81266636966549;0.183997649915341]
Robot Arm 1 togoal: 27.0282397769786
Step 261:
Mobile Platform 1 Pose: [-10.7585697657431;7.87955918571801;4.96425346060612]
Robot Arm 1 end: [8.65260740620164;5.12689190695164;0.778426144321889]
Robot Arm 1 togoal: 5.11019228791763
Mobile Platform 2 Pose: [-3.72416617281436;8.67037679240979;8.71165012333386]
Robot Arm 2 end: [9.36547653080613;8.80888823953932;0.186237883136727]
Robot Arm 1 togoal: 27.0291980639696
Step 262:
Mobile Platform 1 Pose: [-10.9644523593749;8.01595885792071;5.00074491422759]
Robot Arm 1 end: [8.73382038903076;5.24693487567655;0.965754212158154]
Robot Arm 1 togoal: 4.95006431960335
Mobile Platform 2 Pose: [-3.73321742543903;8.67428237311747;8.70827582120317]
Robot Arm 2 end: [9.37059253198547;8.80511398297616;0.188513594018938]
Robot Arm 1 togoal: 27.0301255449371
Step 263:
Mobile Platform 1 Pose: [-11.1532939940598;8.1382125716267;5.03498720968995]
Robot Arm 1 end: [8.75742664517182;5.41101451202904;1.07840984175526]
Robot Arm 1 togoal: 4.77479448763108
Mobile Platform 2 Pose: [-3.74219745169098;8.67814953368685;8.70491040942862]
Robot Arm 2 end: [9.3756661433091;8.80134376826527;0.190824869618603]
Robot Arm 1 togoal: 27.0310253572991
Step 264:
Mobile Platform 1 Pose: [-11.3557575074519;8.26036441847873;5.06476253273435]
Robot Arm 1 end: [8.75691193091691;5.52371747708458;1.17539620074438]
Robot Arm 1 togoal: 4.65852070495717
Mobile Platform 2 Pose: [-3.75111201689405;8.68198185940807;8.70155414979891]
Robot Arm 2 end: [9.38070086670983;8.79757771372551;0.193171829567366]
Robot Arm 1 togoal: 27.0319000745163
Step 265:
Mobile Platform 1 Pose: [-11.5055786158571;8.34531391289312;5.07439342282313]
Robot Arm 1 end: [8.71075702111082;5.55174256960203;1.2696410083828]
Robot Arm 1 togoal: 4.63813787724908
Mobile Platform 2 Pose: [-3.75996600582122;8.68578238515311;8.6982072517597]
Robot Arm 2 end: [9.38569958489511;8.79381589533906;0.195554627735074]
Robot Arm 1 togoal: 27.0327517973765
Step 266:
Mobile Platform 1 Pose: [-11.6262407198647;8.4106019916443;5.07541988618515]
Robot Arm 1 end: [8.6874057854751;5.55867502090479;1.31636956943939]
Robot Arm 1 togoal: 4.63574640754331
Mobile Platform 2 Pose: [-3.76876356039506;8.68955368145085;8.69486987944114]
Robot Arm 2 end: [9.3906646620452;8.79005835072321;0.197973457808825]
Robot Arm 1 togoal: 27.0335822282363
Step 267:
Mobile Platform 1 Pose: [-11.7235931517652;8.46101839834827;5.06927044253788]
Robot Arm 1 end: [8.71513491479284;5.62125389168448;1.27172469306011]
Robot Arm 1 togoal: 4.57016782480936
Mobile Platform 2 Pose: [-3.77750819338431;8.693297925537;8.6915421582443]
Robot Arm 2 end: [9.39559802412597;8.78630508367504;0.20042855405836]
Robot Arm 1 togoal: 27.0343927313752
Step 268:
Mobile Platform 1 Pose: [-11.8193499336179;8.51099977822122;5.06372258623124]
Robot Arm 1 end: [8.74493936167799;5.65932698129715;1.22805114433017]
Robot Arm 1 togoal: 4.52796184758645
Mobile Platform 2 Pose: [-3.78620288324052;8.6970169605957;8.68822418126238]
Robot Arm 2 end: [9.40050122570328;8.78255606891715;0.202920191480746]
Robot Arm 1 togoal: 27.0351843839127
Step 269:
Mobile Platform 1 Pose: [-11.9069308195403;8.55738163266796;5.06089661997555]
Robot Arm 1 end: [8.80671746227956;5.64157032803183;1.21437915730612]
Robot Arm 1 togoal: 4.52922165073596
Mobile Platform 2 Pose: [-3.7948501528437;8.70071234493615;8.6849160130738]
Robot Arm 2 end: [9.40537550686142;8.778811253944;0.205448690055113]
Robot Arm 1 togoal: 27.0359580174746
Step 270:
Mobile Platform 1 Pose: [-11.9911357718781;8.60247414959569;5.06054812368983]
Robot Arm 1 end: [8.90288617450295;5.59604722939696;1.21349124600221]
Robot Arm 1 togoal: 4.54895816485316
Mobile Platform 2 Pose: [-3.80345213515485;8.70438539299283;8.68161769373522]
Robot Arm 2 end: [9.41022183776185;8.77507056192606;0.208014414134825]
Robot Arm 1 togoal: 27.0367142520151
Step 271:
Mobile Platform 1 Pose: [-12.0900466867943;8.65660386075043;5.06517036184151]
Robot Arm 1 end: [8.99618010352225;5.51304881120888;1.26409395613211]
Robot Arm 1 togoal: 4.6050392040171
Mobile Platform 2 Pose: [-3.81201062879272;8.70803721002411;8.6783292421782]
Robot Arm 2 end: [9.41504095702354;8.77133389395565;0.210617773470366]
Robot Arm 1 togoal: 27.0374535247104
Step 272:
Mobile Platform 1 Pose: [-12.2424109519607;8.7428196781168;5.08814030681858]
Robot Arm 1 end: [9.09423055012795;5.42971758046759;1.3372563934855]
Robot Arm 1 togoal: 4.66279544946414
Mobile Platform 2 Pose: [-3.8205271447499;8.7116687212771;8.67505065815342]
Robot Arm 2 end: [9.41983340430574;8.76760112973179;0.213259226170793]
Robot Arm 1 togoal: 27.0381761135479
Step 273:
Mobile Platform 1 Pose: [-12.4660385789181;8.87790860890125;5.17388298498534]
Robot Arm 1 end: [9.21479453207416;5.40798309031885;1.40419975913317]
Robot Arm 1 togoal: 4.66012974303557
Mobile Platform 2 Pose: [-3.82900294595729;8.71528069669061;8.67178192546048]
Robot Arm 2 end: [9.42459954575915;8.76387213022207;0.215939277265097]
Robot Arm 1 togoal: 27.0388821574128
Step 274:
Mobile Platform 1 Pose: [-12.7644542073913;9.07351541200506;5.21627889266984]
Robot Arm 1 end: [9.25505462392797;5.46175043814575;1.46007034408976]
Robot Arm 1 togoal: 4.59938747247617
Mobile Platform 2 Pose: [-3.83743908085075;8.71887377184794;8.66852301406456]
Robot Arm 2 end: [9.42933959623484;8.760146738993;0.218658479661185]
Robot Arm 1 togoal: 27.0395716728273
Step 275:
Mobile Platform 1 Pose: [-12.8755998557928;9.17708534342308;5.41780901824877]
Robot Arm 1 end: [9.32182818773643;5.63066159521587;1.47778330436533]
Robot Arm 1 togoal: 4.42186643683998
Mobile Platform 2 Pose: [-3.84583641184253;8.72244846574903;8.66527388022199]
Robot Arm 2 end: [9.43405363827668;8.75642478130246;0.221417437668088]
Robot Arm 1 togoal: 27.0402445667266
Step 276:
Mobile Platform 1 Pose: [-13.0994055870766;9.33285508578356;5.52058851000746]
Robot Arm 1 end: [9.37127810806993;5.73532702762734;1.48986673082963]
Robot Arm 1 togoal: 4.31088112328875
Mobile Platform 2 Pose: [-3.85419564016002;8.72600519631737;8.6620344692154]
Robot Arm 2 end: [9.43874163718517;8.75270606644262;0.224216805951014]
Robot Arm 1 togoal: 27.0409006489059
Step 277:
Mobile Platform 1 Pose: [-12.8955771119043;9.18551996774445;5.6946899868839]
Robot Arm 1 end: [9.3187268878041;5.87478431151828;1.48832823757186]
Robot Arm 1 togoal: 4.18122051247682
Mobile Platform 2 Pose: [-3.86251732674299;8.72954429344283;8.65880471596877]
Robot Arm 2 end: [9.44340345397002;8.74899038762159;0.227057291633827]
Robot Arm 1 togoal: 27.0415396411061
Step 278:
Mobile Platform 1 Pose: [-13.1420458651953;9.34378695884443;5.73799942456879]
Robot Arm 1 end: [9.40742620684184;5.92055958533814;1.49714604726862]
Robot Arm 1 togoal: 4.12232298688194
Mobile Platform 2 Pose: [-3.87080191045055;8.73306601034477;8.65558454662776]
Robot Arm 2 end: [9.44803885539948;8.74527752318103;0.22993965391318]
Robot Arm 1 togoal: 27.0421611850725
Step 279:
Mobile Platform 1 Pose: [-13.1562745579222;9.34709208427381;5.95564591623173]
Robot Arm 1 end: [9.42330128831341;6.07014799545833;1.50875417774648]
Robot Arm 1 togoal: 3.9719602392048
Mobile Platform 2 Pose: [-3.87904972384912;8.73657053342001;8.65237387904254]
Robot Arm 2 end: [9.45264752313378;8.74156723641941;0.232864706170154]
Robot Arm 1 togoal: 27.0427648488782
Step 280:
Mobile Platform 1 Pose: [-13.2044512654445;9.39472746136815;6.2010657011909]
Robot Arm 1 end: [9.37001846310145;6.22297091553006;1.51984185558162]
Robot Arm 1 togoal: 3.82920706975523
Mobile Platform 2 Pose: [-3.88726100702135;8.74005799085436;8.64917262312663]
Robot Arm 2 end: [9.45722906098149;8.73785927538092;0.235833317637311]
Robot Arm 1 togoal: 27.0433501319074
Step 281:
Mobile Platform 1 Pose: [-13.3361692070901;9.38263009695508;6.45077283720079]
Robot Arm 1 end: [9.30137320815702;6.34740849163222;1.50985963190718]
Robot Arm 1 togoal: 3.71882081163942
Mobile Platform 2 Pose: [-3.89543591985868;8.74352846028436;8.64598068167723]
Robot Arm 2 end: [9.4617830014719;8.73415337305437;0.238846415510607]
Robot Arm 1 togoal: 27.0439164696189
Step 282:
Mobile Platform 1 Pose: [-13.4326554161997;9.31911757735394;6.69257060363813]
Robot Arm 1 end: [9.12831123036985;6.59939346658086;1.48862146697597]
Robot Arm 1 togoal: 3.51069999795384
Mobile Platform 2 Pose: [-3.90357455256886;8.74698197534382;8.64279795202558]
Robot Arm 2 end: [9.46630880856887;8.73044924916511;0.241904981775336]
Robot Arm 1 togoal: 27.044463236895
Step 283:
Mobile Platform 1 Pose: [-13.6746164982625;9.48357528025021;6.7198693590559]
Robot Arm 1 end: [9.31795197502649;6.61858333672551;1.4925829127588]
Robot Arm 1 togoal: 3.44963413855556
Mobile Platform 2 Pose: [-3.91167693541514;8.7504185317162;8.63962432478191]
Robot Arm 2 end: [9.47080588351598;8.72674660772531;0.245010059820915]
Robot Arm 1 togoal: 27.0449897502467
Step 284:
Mobile Platform 1 Pose: [-13.8854595188461;9.5789228904959;6.77305677728918]
Robot Arm 1 end: [9.51424066907677;6.75405664864796;1.5177554497009]
Robot Arm 1 togoal: 3.2820909333643
Mobile Platform 2 Pose: [-3.91974304733486;8.75383809249277;8.6364596851512]
Robot Arm 2 end: [9.47527356599278;8.72304513845677;0.248162751694511]
Robot Arm 1 togoal: 27.0454952697787
Step 285:
Mobile Platform 1 Pose: [-13.7279380169903;9.45732148636502;6.77896664266371]
Robot Arm 1 end: [9.51408889012701;6.80458212829731;1.51511525210993]
Robot Arm 1 togoal: 3.23215711433597
Mobile Platform 2 Pose: [-3.92777282415101;8.75724059326172;8.63330391305568]
Robot Arm 2 end: [9.47971113734614;8.71934451622392;0.251364221111803]
Robot Arm 1 togoal: 27.0459790010005
Step 286:
Mobile Platform 1 Pose: [-13.3426174121981;9.20755930383626;6.78986857729371]
Robot Arm 1 end: [9.40170826292824;6.81571789142729;1.48142564063879]
Robot Arm 1 togoal: 3.24024253406183
Mobile Platform 2 Pose: [-3.9357661657993;8.76062594658084;8.63015688318885]
Robot Arm 2 end: [9.48411782159667;8.71564440074132;0.254615694126829]
Robot Arm 1 togoal: 27.0464400953438
Step 287:
Mobile Platform 1 Pose: [-12.9186471653953;8.95795310310205;6.77584200483467]
Robot Arm 1 end: [9.27441791200345;6.87503547582822;1.42582969170655]
Robot Arm 1 togoal: 3.20950619742886
Mobile Platform 2 Pose: [-3.94372294336106;8.76399404631938;8.62701846615536]
Robot Arm 2 end: [9.48849278668638;8.7119444373889;0.257918459561202]
Robot Arm 1 togoal: 27.0468776516339
Step 288:
Mobile Platform 1 Pose: [-12.4803982745717;8.70880828768072;6.75518051703523]
Robot Arm 1 end: [9.10028807414764;6.99075489225326;1.33601280327604]
Robot Arm 1 togoal: 3.14630861979088
Mobile Platform 2 Pose: [-3.95164300522703;8.76734477145032;8.62388852671742]
Robot Arm 2 end: [9.49283514456822;8.70824425477826;0.261273872194015]
Robot Arm 1 togoal: 27.047290714355
Step 289:
Mobile Platform 1 Pose: [-12.0297546732665;8.45942747094312;6.77276486082664]
Robot Arm 1 end: [8.84181557552821;7.22366133450518;1.18033457535201]
Robot Arm 1 togoal: 3.02745775827859
Mobile Platform 2 Pose: [-3.95952618385726;8.77067799020564;8.62076692598855]
Robot Arm 2 end: [9.49714395081935;8.70454346708994;0.26468334980908]
Robot Arm 1 togoal: 27.0476782751639
Step 290:
Mobile Platform 1 Pose: [-11.778476985798;8.31136433847121;6.8014748032191]
Robot Arm 1 end: [8.68792601799185;7.36250409359187;1.03370350092869]
Robot Arm 1 togoal: 2.98586347119046
Mobile Platform 2 Pose: [-3.96737230179811;8.77399356375447;8.6176535197877]
Robot Arm 2 end: [9.50141820484988;8.70084167157782;0.26814837749721]
Robot Arm 1 togoal: 27.0480392711538
Step 291:
Mobile Platform 1 Pose: [-11.6686994543357;8.25438095678295;6.79251049192947]
Robot Arm 1 end: [8.67585230053444;7.38372774585503;0.851305927955406]
Robot Arm 1 togoal: 3.00777954443813
Mobile Platform 2 Pose: [-3.97518117796546;8.77729135004104;8.61454816017633]
Robot Arm 2 end: [9.50565684904535;8.69713845006823;0.27167050602148]
Robot Arm 1 togoal: 27.0483725853933
Step 292:
Mobile Platform 1 Pose: [-11.6347203286107;8.24325547591634;6.75734958036814]
Robot Arm 1 end: [8.74061063266279;7.33237745325425;0.644903035777539]
Robot Arm 1 togoal: 3.07730686171985
Mobile Platform 2 Pose: [-3.98295263342418;8.78057120729573;8.61145069392128]
Robot Arm 2 end: [9.50985876704118;8.69343336696027;0.27525135334464]
Robot Arm 1 togoal: 27.0486770442756
Step 293:
Mobile Platform 1 Pose: [-11.6422222005932;8.24681205783191;6.70456467156368]
Robot Arm 1 end: [8.62905663522978;7.32652438446605;0.410818515006674]
Robot Arm 1 togoal: 3.2030392607466
Mobile Platform 2 Pose: [-3.99068649803958;8.78383299807633;8.60836096396576]
Robot Arm 2 end: [9.51402278248377;8.68972597073756;0.278892602377463]
Robot Arm 1 togoal: 27.0489514178237
Step 294:
Mobile Platform 1 Pose: [-11.6440629733612;8.24810293085173;6.58957531737088]
Robot Arm 1 end: [8.4366806878887;7.21231327863853;0.235747869544119]
Robot Arm 1 togoal: 3.44485668307356
Mobile Platform 2 Pose: [-3.9983826167458;8.787076593056;8.60527880782784]
Robot Arm 2 end: [9.51814765800727;8.68601579161527;0.282596004372374]
Robot Arm 1 togoal: 27.0491944172217
Step 295:
Mobile Platform 1 Pose: [-11.3681879536837;8.09805413487939;6.58161112202148]
Robot Arm 1 end: [8.09637536326217;7.19138391192975;0.140665028127366]
Robot Arm 1 togoal: 3.66298180753562
Mobile Platform 2 Pose: [-4.00604085623671;8.79030187506516;8.60220405861465]
Robot Arm 2 end: [9.52223209245875;8.68230234287276;0.286363374716574]
Robot Arm 1 togoal: 27.0494046939822
Step 296:
Mobile Platform 1 Pose: [-10.9433192141979;7.86544326662915;6.5808186161761]
Robot Arm 1 end: [7.74651067323716;7.21212404432417;0.112262242863887]
Robot Arm 1 togoal: 3.85162414553268
Mobile Platform 2 Pose: [-4.01366111200045;8.79350874332728;8.59913654467809]
Robot Arm 2 end: [9.52627471967313;8.67858512008127;0.290196593894081]
Robot Arm 1 togoal: 27.0495808385407
Step 297:
Mobile Platform 1 Pose: [-10.4807414197626;7.61557096251577;6.57282914577703]
Robot Arm 1 end: [7.41566605050157;7.26347441513179;0.163556251987473]
Robot Arm 1 togoal: 4.00125077546844
Mobile Platform 2 Pose: [-4.02124331539141;8.79669711770602;8.59607608922846]
Robot Arm 2 end: [9.53027410560094;8.67486360076286;0.294097605144119]
Robot Arm 1 togoal: 27.0497213781123
Step 298:
Mobile Platform 1 Pose: [-10.0239201012473;7.36655306655045;6.55121790069123]
Robot Arm 1 end: [7.10510940530102;7.26137125059347;0.193609891950299]
Robot Arm 1 togoal: 4.20029088055825
Mobile Platform 2 Pose: [-4.02878744145201;8.79986694340035;8.59302250973511]
Robot Arm 2 end: [9.53422874662428;8.67113724347742;0.298068414483912]
Robot Arm 1 togoal: 27.049824774905
Step 299:
Mobile Platform 1 Pose: [-9.61389449592815;7.14139169798858;6.51953828282901]
Robot Arm 1 end: [6.80248957868765;7.26204989783511;0.311772261577303]
Robot Arm 1 togoal: 4.37980312111363
Mobile Platform 2 Pose: [-4.0362935169992;8.80301819579012;8.58997561822753]
Robot Arm 2 end: [9.53813706600245;8.66740548869133;0.302111084619856]
Robot Arm 1 togoal: 27.0498894244904
Step 300:
Mobile Platform 1 Pose: [-9.20129321679093;6.91441960395322;6.51066979595203]
Robot Arm 1 end: [6.49332504234342;7.24838543238907;0.465110428667506]
Robot Arm 1 togoal: 4.5807264461211
Mobile Platform 2 Pose: [-4.04376162960268;8.80615088581228;8.58693522081201]
Robot Arm 2 end: [9.54199741284114;8.66366775793448;0.306227735143815]
Robot Arm 1 togoal: 27.0499136545298
Step 301:
Mobile Platform 1 Pose: [-8.77575597209462;6.68074934954615;6.52079770237892]
Robot Arm 1 end: [6.23494101574464;7.24249089056802;0.639059107562868]
Robot Arm 1 togoal: 4.74945737756169
Mobile Platform 2 Pose: [-4.05119193652069;8.80926506529761;8.58390111732156]
Robot Arm 2 end: [9.5458080586297;8.65992345395957;0.310420536758577]
Robot Arm 1 togoal: 27.0498957227056
Step 302:
Mobile Platform 1 Pose: [-8.39295595922209;6.47022042506134;6.53663652111684]
Robot Arm 1 end: [6.1069004412719;7.2823302948447;0.767074922238346]
Robot Arm 1 togoal: 4.80732314530298
Mobile Platform 2 Pose: [-4.05858467467137;8.81236083292769;8.58087310112366]
Robot Arm 2 end: [9.54956719514513;8.65617196083035;0.31469170697999]
Robot Arm 1 togoal: 27.0498338155223
Step 303:
Mobile Platform 1 Pose: [-8.03975235590459;6.27611905064296;6.56827877790789]
Robot Arm 1 end: [6.03394674103467;7.31474588756797;0.882172264224582]
Robot Arm 1 togoal: 4.83200465410358
Mobile Platform 2 Pose: [-4.06594017105875;8.81543834045477;8.57785095793668]
Robot Arm 2 end: [9.55327293158192;8.65241264319492;0.319043504589775]
Robot Arm 1 togoal: 27.0497260460399
Step 304:
Mobile Platform 1 Pose: [-7.64955561339472;6.06353546361194;6.62191092308018]
Robot Arm 1 end: [5.95125494213692;7.4020078486984;0.875397716033834]
Robot Arm 1 togoal: 4.85373073736665
Mobile Platform 2 Pose: [-4.0732588543152;8.81849779959262;8.57483446649526]
Robot Arm 2 end: [9.55692329313995;8.64864484759166;0.323478222772456]
Robot Arm 1 togoal: 27.0495704541181
Step 305:
Mobile Platform 1 Pose: [-7.26184316850347;5.85290994287787;6.69677376465229]
Robot Arm 1 end: [5.8360971790859;7.48461689441076;0.869875468774371]
Robot Arm 1 togoal: 4.90807518535158
Mobile Platform 2 Pose: [-4.08054126636565;8.82153948896186;8.57182339726328]
Robot Arm 2 end: [9.56051621852674;8.64486790181905;0.327998182141885]
Robot Arm 1 togoal: 27.049365004542
Step 306:
Mobile Platform 1 Pose: [-6.94199965910931;5.67619730964843;6.80060318007018]
Robot Arm 1 end: [5.80123851220536;7.55508676722112;0.90645433817316]
Robot Arm 1 togoal: 4.89743454450133
Mobile Platform 2 Pose: [-4.08778807521812;8.82456376171167;8.56881751223596]
Robot Arm 2 end: [9.56404955765277;8.64108111605943;0.33260571921567]
Robot Arm 1 togoal: 27.0491075866913
Step 307:
Mobile Platform 1 Pose: [-6.69783833183905;5.55010222190273;6.95094665878999]
Robot Arm 1 end: [5.76476743831268;7.66544724188689;0.92495121898075]
Robot Arm 1 togoal: 4.87263846199825
Mobile Platform 2 Pose: [-4.09500008880448;8.82757105376675;8.56581656337264]
Robot Arm 2 end: [9.56752107153462;8.63728378161339;0.337303181732373]
Robot Arm 1 togoal: 27.0487960138509
Step 308:
Mobile Platform 1 Pose: [-6.43173503223867;5.426316446818;7.11324918163262]
Robot Arm 1 end: [5.81930662846883;7.78779304986086;0.82573770335856]
Robot Arm 1 togoal: 4.78073700574919
Mobile Platform 2 Pose: [-4.10217826917817;8.83056189227816;8.56282029359569]
Robot Arm 2 end: [9.57092857679454;8.63347328620209;0.342092826970302]
Robot Arm 1 togoal: 27.0484268338885
Step 309:
Mobile Platform 1 Pose: [-6.43929961923818;5.443866565831;7.20828163784271]
Robot Arm 1 end: [5.63927811764117;7.89239215378535;0.963065125300961]
Robot Arm 1 togoal: 4.87536639578136
Mobile Platform 2 Pose: [-4.10932414900887;8.83353715442124;8.55982845033316]
Robot Arm 2 end: [9.57427164088896;8.629623500621;0.346975455923382]
Robot Arm 1 togoal: 27.0479817301826
Step 310:
Mobile Platform 1 Pose: [-6.12628266089746;5.28419859070442;7.27707029094947]
Robot Arm 1 end: [5.68627438035466;7.9428465774371;0.835912741329201]
Robot Arm 1 togoal: 4.82798648274204
Mobile Platform 2 Pose: [-4.11644473511119;8.83650111815588;8.55684097199344]
Robot Arm 2 end: [9.57754919311345;8.62573088620657;0.351948844540169]
Robot Arm 1 togoal: 27.0474576265173
Step 311:
Mobile Platform 1 Pose: [-6.01128091494536;5.26983023845374;7.46872446980958]
Robot Arm 1 end: [5.740200413295;8.10300853390518;0.741436109734003]
Robot Arm 1 togoal: 4.72781016965945
Mobile Platform 2 Pose: [-4.12354888758474;8.83945919901751;8.55385782401207]
Robot Arm 2 end: [9.58076123312577;8.62179244157416;0.357008983375539]
Robot Arm 1 togoal: 27.046852668888
Step 312:
Mobile Platform 1 Pose: [-6.04881124577676;5.3623527564001;7.66092577094409]
Robot Arm 1 end: [5.75958283404725;8.31876647490938;0.802604393590588]
Robot Arm 1 togoal: 4.61776744266142
Mobile Platform 2 Pose: [-4.13064707336972;8.84241779548338;8.55087898498499]
Robot Arm 2 end: [9.58390880842074;8.61780578191493;0.362150140557117]
Robot Arm 1 togoal: 27.0461662596823
Step 313:
Mobile Platform 1 Pose: [-5.85294413428035;5.31192639351988;7.81274132343533]
Robot Arm 1 end: [5.7852884958693;8.47264185234064;0.649455902482704]
Robot Arm 1 togoal: 4.5668594338112
Mobile Platform 2 Pose: [-4.13775110400471;8.84538412513021;8.54790443347827]
Robot Arm 2 end: [9.5869939857447;8.61376921851789;0.367364955700568]
Robot Arm 1 togoal: 27.0453990862268
Step 314:
Mobile Platform 1 Pose: [-6.04815080734416;5.48274907367475;7.95763770702608]
Robot Arm 1 end: [5.87077830446668;8.62332047204568;0.794089534040953]
Robot Arm 1 togoal: 4.41294880539899
Mobile Platform 2 Pose: [-4.14487386157856;8.84836605414253;8.54493413781047]
Robot Arm 2 end: [9.59001981742261;8.60968183927777;0.372644567106422]
Robot Arm 1 togoal: 27.0445531448832
Step 315:
Mobile Platform 1 Pose: [-5.90741643015857;5.45404418173344;8.05025727583823]
Robot Arm 1 end: [5.91898321925303;8.71593492663996;0.648654308473031]
Robot Arm 1 togoal: 4.36629225807059
Mobile Platform 2 Pose: [-4.15202901540284;8.85137192178654;8.54196804332503]
Robot Arm 2 end: [9.59299029792727;8.60554357580341;0.37797877455728]
Robot Arm 1 togoal: 27.0436317473745
Step 316:
Mobile Platform 1 Pose: [-6.07385393936392;5.60348507751245;8.18982523533617]
Robot Arm 1 end: [5.98651565631244;8.86297055714507;0.789823459839963]
Robot Arm 1 togoal: 4.23503384447215
Mobile Platform 2 Pose: [-4.15923073504213;8.85441036335114;8.53900606377953]
Robot Arm 2 end: [9.59591031287762;8.60135526457486;0.383356233712673]
Robot Arm 1 togoal: 27.0426395162943
Step 317:
Mobile Platform 1 Pose: [-5.93418971732258;5.57673093555639;8.2844418593214]
Robot Arm 1 end: [6.03645914393413;8.95516041964562;0.649258292816217]
Robot Arm 1 togoal: 4.19061802964935
Mobile Platform 2 Pose: [-4.16649340211656;8.85749013301612;8.53604807281392]
Robot Arm 2 end: [9.598785574627;8.5971186921901;0.388764675980221]
Robot Arm 1 togoal: 27.0415823590368
Step 318:
Mobile Platform 1 Pose: [-6.06946783900852;5.71289272132584;8.42633850148271]
Robot Arm 1 end: [6.09797979556129;9.11902439468698;0.754646934821157]
Robot Arm 1 togoal: 4.07298130532336
Mobile Platform 2 Pose: [-4.17383132615765;8.8606199298365;8.53309389790137]
Robot Arm 2 end: [9.60162254624901;8.59283662220646;0.394191159688323]
Robot Arm 1 togoal: 27.04046741956
Step 319:
Mobile Platform 1 Pose: [-5.93219315219675;5.69651757681936;8.53568558642219]
Robot Arm 1 end: [6.1397609802217;9.19807485173859;0.754123588341622]
Robot Arm 1 togoal: 4.01654436848282
Mobile Platform 2 Pose: [-4.18125846739931;8.86380822851456;8.53014331495021]
Robot Arm 2 end: [9.60442834871425;8.58851279872907;0.399622341269887]
Robot Arm 1 togoal: 27.0393030014068
Step 320:
Mobile Platform 1 Pose: [-6.05176057389853;5.82762325163115;8.67892982420953]
Robot Arm 1 end: [6.2037539780168;9.36838717871221;0.797479147538997]
Robot Arm 1 togoal: 3.9158525303418
Mobile Platform 2 Pose: [-4.18878817123856;8.86706311773722;8.52719604436271]
Robot Arm 2 end: [9.60721065154473;8.58415192219486;0.405044766978302]
Robot Arm 1 togoal: 27.0380984590887
Step 321:
Mobile Platform 1 Pose: [-5.91853453290254;5.81896682237207;8.79489404072604]
Robot Arm 1 end: [6.22217323561532;9.41369722675975;0.921307638154754]
Robot Arm 1 togoal: 3.86980063213416
Mobile Platform 2 Pose: [-4.19643291800063;8.87039214815396;8.52425175060671]
Robot Arm 2 end: [9.60997754548034;8.57975960151955;0.410445170971286]
Robot Arm 1 togoal: 27.0368640602563
Step 322:
Mobile Platform 1 Pose: [-6.10007939662256;5.96542431651528;8.89901931846702]
Robot Arm 1 end: [6.31619711816062;9.57911810884514;0.869541378363551]
Robot Arm 1 togoal: 3.7645641944659
Mobile Platform 2 Pose: [-4.20420409131322;8.87380219184188;8.52131004052691]
Robot Arm 2 end: [9.61273739549865;8.57534226824548;0.415810786533686]
Robot Arm 1 togoal: 27.0356108073189
Step 323:
Mobile Platform 1 Pose: [-5.93895101117765;5.9470882023246;9.02964488082041]
Robot Arm 1 end: [6.3330128004386;9.60432750418719;1.00510869859722]
Robot Arm 1 togoal: 3.72417718168093
Mobile Platform 2 Pose: [-4.21211176916904;8.87729931558915;8.51837046614501]
Robot Arm 2 end: [9.61549867597353;8.57090706766731;0.421129647387908]
Robot Arm 1 togoal: 27.0343502310599
Step 324:
Mobile Platform 1 Pose: [-6.13691235889857;6.09085290066766;9.10676678118588]
Robot Arm 1 end: [6.37936559896067;9.78937023436187;0.926889265285266]
Robot Arm 1 togoal: 3.67509536328862
Mobile Platform 2 Pose: [-4.22016454027255;8.88088866940425;8.51543252594923]
Robot Arm 2 end: [9.61826978736196;8.56646171175515;0.426390888089]
Robot Arm 1 togoal: 27.0330941449858
Step 325:
Mobile Platform 1 Pose: [-5.93293389139022;6.04319528641738;9.24248374012549]
Robot Arm 1 end: [6.44928160935757;9.76847757154613;1.04272940499489]
Robot Arm 1 togoal: 3.59025716162089
Mobile Platform 2 Pose: [-4.22836934922084;8.88457439228183;8.51249566920888]
Robot Arm 2 end: [9.62105885800257;8.56201431081839;0.431585020208203]
Robot Arm 1 togoal: 27.0318543755774
Step 326:
Mobile Platform 1 Pose: [-6.1774412161247;6.21738014309732;9.32121431643507]
Robot Arm 1 end: [6.48273607232124;9.96778128586892;0.991238923879623]
Robot Arm 1 togoal: 3.55708172246052
Mobile Platform 2 Pose: [-4.23673137291576;8.88835953656427;8.50955930120359]
Robot Arm 2 end: [9.62387353455315;8.55757317758272;0.436704189479011]
Robot Arm 1 togoal: 27.030642466397
Step 327:
Mobile Platform 1 Pose: [-6.09276416997946;6.24587933394665;9.48381756601165]
Robot Arm 1 end: [6.540398028219;10.0250196386053;1.10103094995634]
Robot Arm 1 togoal: 3.48508906463831
Mobile Platform 2 Pose: [-4.24525393029776;8.89224601210152;8.50662278762168]
Robot Arm 2 end: [9.62672076368608;8.55314661181673;0.441742402045852]
Robot Arm 1 togoal: 27.0294693639741
Step 328:
Mobile Platform 1 Pose: [-6.1719404840394;6.36638832639869;9.66895990845182]
Robot Arm 1 end: [6.48828181015667;10.2019105959828;1.17116946713907]
Robot Arm 1 togoal: 3.53487113419548
Mobile Platform 2 Pose: [-4.25393842761369;8.89623455164281;8.50368546006314]
Robot Arm 2 end: [9.62960657253216;8.54874267180405;0.446695711306367]
Robot Arm 1 togoal: 27.0283450955087
Step 329:
Mobile Platform 1 Pose: [-6.33617888755259;6.50944751592173;9.81437984286789]
Robot Arm 1 end: [6.63338458191188;10.4261030914032;1.09738047513504]
Robot Arm 1 togoal: 3.41981238076869
Mobile Platform 2 Pose: [-4.26278434189168;8.90032469874249;8.50074662381259]
Robot Arm 2 end: [9.63253585529706;8.5443689456296;0.451562361815404]
Robot Arm 1 togoal: 27.0272784527618
Step 330:
Mobile Platform 1 Pose: [-6.20490197032353;6.45862440459629;9.85886824073372]
Robot Arm 1 end: [6.68093164011569;10.3337134306663;1.1877858196218]
Robot Arm 1 togoal: 3.35240378257114
Mobile Platform 2 Pose: [-4.27178924231566;8.90451481833186;8.49780556179345]
Robot Arm 2 end: [9.6355121693019;8.54003231607931;0.456342891464535]
Robot Arm 1 togoal: 27.0262766801279
Step 331:
Mobile Platform 1 Pose: [-6.50920705619241;6.68313250533362;9.96885056978413]
Robot Arm 1 end: [6.73554091869731;10.5758146608742;1.09893871507678]
Robot Arm 1 togoal: 3.34161508037779
Mobile Platform 2 Pose: [-4.28094884872817;8.90880213081213;8.49486154137978]
Robot Arm 2 end: [9.63853755274809;8.53573874573857;0.461040175432198]
Robot Arm 1 togoal: 27.0253451946625
Step 332:
Mobile Platform 1 Pose: [-6.34608916673315;6.5860270139814;9.95203628274541]
Robot Arm 1 end: [6.77155449390556;10.5342101979456;1.10185803059885]
Robot Arm 1 togoal: 3.29907879747119
Mobile Platform 2 Pose: [-4.29025713502975;8.91318277111927;8.49191381850906]
Robot Arm 2 end: [9.6416123683723;8.53149307348134;0.465659424520374]
Robot Arm 1 togoal: 27.0244873344923
Step 333:
Mobile Platform 1 Pose: [-6.32597909773468;6.57664375470761;9.9561004995153]
Robot Arm 1 end: [6.73150419051735;10.6295318528048;1.01035136564709]
Robot Arm 1 togoal: 3.36751202493293
Mobile Platform 2 Pose: [-4.29970646567185;8.91765187097977;8.48896164328034]
Robot Arm 2 end: [9.64473518331039;8.5272988447303;0.470208122944583]
Robot Arm 1 togoal: 27.0237041590013
Step 334:
Mobile Platform 1 Pose: [-6.39937975939641;6.62594612966355;9.97661897094999]
Robot Arm 1 end: [6.63391130327928;10.7449674288996;0.890553578410967]
Robot Arm 1 togoal: 3.50471001294069
Mobile Platform 2 Pose: [-4.30928777683502;8.92220366613448;8.48600426288686]
Robot Arm 2 end: [9.64790268893468;8.52315817109946;0.474695913985425]
Robot Arm 1 togoal: 27.0229943001479
Step 335:
Mobile Platform 1 Pose: [-6.52169519891137;6.71907320105681;10.0385545192858]
Robot Arm 1 end: [6.57124004254049;10.8478283752722;0.756835948049823]
Robot Arm 1 togoal: 3.61374521764987
Mobile Platform 2 Pose: [-4.31899078881954;8.92683162597975;8.48304092579641]
Robot Arm 2 end: [9.65110966949727;8.51907163472529;0.479134426886233]
Robot Arm 1 togoal: 27.0223538820573
Step 336:
Mobile Platform 1 Pose: [-6.59897658031705;6.80568012665317;10.1487055002001]
Robot Arm 1 end: [6.54364285402869;10.9541849332657;0.613255890801691]
Robot Arm 1 togoal: 3.69876641262494
Mobile Platform 2 Pose: [-4.32880425938846;8.93152860637224;8.48007088320568]
Robot Arm 2 end: [9.65434902029247;8.5150382298258;0.483537055233972]
Robot Arm 1 togoal: 27.0217765038708
Step 337:
Mobile Platform 1 Pose: [-6.44917189937807;6.7497185504228;10.2220887018619]
Robot Arm 1 end: [6.5808012955325;11.0245564758454;0.767755522648178]
Robot Arm 1 togoal: 3.64801493904426
Mobile Platform 2 Pose: [-4.33871626469326;8.93628702226339;8.47709339117739]
Robot Arm 2 end: [9.65761182150058;8.51105535630611;0.48791867576494]
Robot Arm 1 togoal: 27.0212533009021
Step 338:
Mobile Platform 1 Pose: [-6.30094676874298;6.6848901729846;10.2621801027841]
Robot Arm 1 end: [6.66851561338326;11.0278140148775;0.893449123943074]
Robot Arm 1 togoal: 3.54245818175593
Mobile Platform 2 Pose: [-4.34871450971882;8.94109903874683;8.47410771151618]
Robot Arm 2 end: [9.66088746597898;8.50711885514655;0.492295325600453]
Robot Arm 1 togoal: 27.0207730750906
Step 339:
Mobile Platform 1 Pose: [-6.23704221325137;6.66997364398609;10.3035018447033]
Robot Arm 1 end: [6.74194002914914;10.9823861160911;1.02507800171842]
Robot Arm 1 togoal: 3.43889166493056
Mobile Platform 2 Pose: [-4.35878666041758;8.94595677679498;8.47111311216185]
Robot Arm 2 end: [9.66416384116766;8.50322309406818;0.496683830977922]
Robot Arm 1 togoal: 27.0203225009485
Step 340:
Mobile Platform 1 Pose: [-6.27749338607148;6.73225261158409;10.3839213243739]
Robot Arm 1 end: [6.84853167942448;10.9845140418754;1.11406649662708]
Robot Arm 1 togoal: 3.32665227374432
Mobile Platform 2 Pose: [-4.36892069293895;8.95085253004686;8.46810886655302]
Robot Arm 2 end: [9.66742756141366;8.49936108713074;0.501101400263691]
Robot Arm 1 togoal: 27.0198863926043
Step 341:
Mobile Platform 1 Pose: [-6.45277408867209;6.84144286173265;10.4058986120781]
Robot Arm 1 end: [6.8720012966931;11.0919778991491;1.01968572578196]
Robot Arm 1 togoal: 3.35083685392984
Mobile Platform 2 Pose: [-4.37910525013973;8.95577898767149;8.46509425281883]
Robot Arm 2 end: [9.67066424684036;8.49552465601882;0.50556518710519]
Robot Arm 1 togoal: 27.019448034195
Step 342:
Mobile Platform 1 Pose: [-6.43357680659223;6.82540290271643;10.3950031711749]
Robot Arm 1 end: [6.98105548035358;10.9987073520075;1.10043170277766]
Robot Arm 1 togoal: 3.20754112682682
Mobile Platform 2 Pose: [-4.38932999754014;8.96072945763249;8.46206855228217]
Robot Arm 2 end: [9.67385884164862;8.49170462077299;0.51009183467256]
Robot Arm 1 togoal: 27.0189895595997
Step 343:
Mobile Platform 1 Pose: [-6.48809403254259;6.85726241144142;10.3982020483047]
Robot Arm 1 end: [7.0819716655349;11.0129815042647;1.06313027223972]
Robot Arm 1 togoal: 3.1226055748882
Mobile Platform 2 Pose: [-4.39958597089618;8.96569808449607;8.45903104664792]
Robot Arm 2 end: [9.67699596426067;8.48789101468927;0.514697010450492]
Robot Arm 1 togoal: 27.018492371991
Step 344:
Mobile Platform 1 Pose: [-6.5589503295896;6.89837500009134;10.4017250632899]
Robot Arm 1 end: [7.20284302064025;11.0139589746674;1.02014881922287]
Robot Arm 1 togoal: 3.0171264267139
Mobile Platform 2 Pose: [-4.40986590019639;8.9706800542227;8.45598101529165]
Robot Arm 2 end: [9.68006028095319;8.48407331591253;0.519394948536686]
Robot Arm 1 togoal: 27.0179375913318
Step 345:
Mobile Platform 1 Pose: [-6.64355729943735;6.94727888363228;10.4055102230877]
Robot Arm 1 end: [7.35214635244152;10.9892754549083;0.969201245859863]
Robot Arm 1 togoal: 2.87997859881691
Mobile Platform 2 Pose: [-4.42016450692628;8.9756717801518;8.45291773000586]
Robot Arm 2 end: [9.68303689132422;8.48024069313491;0.524198013919929]
Robot Arm 1 togoal: 27.0173065189867
Step 346:
Mobile Platform 1 Pose: [-6.75128187721944;7.00978269167414;10.4104924473109]
Robot Arm 1 end: [7.53553812108621;10.9311898790192;0.891634521067179]
Robot Arm 1 togoal: 2.70865055876157
Mobile Platform 2 Pose: [-4.43047875099562;8.98067106068575;8.44984045093801]
Robot Arm 2 end: [9.68591171895466;8.47638225238602;0.529116297608952]
Robot Arm 1 togoal: 27.0165811053477
Step 347:
Mobile Platform 1 Pose: [-6.90302958386206;7.09981209170354;10.4198232377501]
Robot Arm 1 end: [7.73093770647742;10.8604233321084;0.756933773029644]
Robot Arm 1 togoal: 2.54416375288997
Mobile Platform 2 Pose: [-4.44080803005045;8.98567720446333;8.44674841963619]
Robot Arm 2 end: [9.68867189185776;8.47248728701678;0.534157269055774]
Robot Arm 1 togoal: 27.0157444096927
Step 348:
Mobile Platform 1 Pose: [-7.11040057054803;7.23560003478703;10.4501398620888]
Robot Arm 1 end: [7.92212885197013;10.8073926650801;0.645094317160631]
Robot Arm 1 togoal: 2.39512884594346
Mobile Platform 2 Pose: [-4.45115431584711;8.99069111584365;8.44364085092828]
Robot Arm 2 end: [9.6913044653089;8.46856775212158;0.539333285351642]
Robot Arm 1 togoal: 27.0147947595174
Step 349:
Mobile Platform 1 Pose: [-7.08981504402698;7.27264894956946;10.5298925125897]
Robot Arm 1 end: [7.98821441146705;10.8100470145425;0.688847843319891]
Robot Arm 1 togoal: 2.32291511507964
Mobile Platform 2 Pose: [-4.4615309940703;8.99572124534503;8.44051767780548]
Robot Arm 2 end: [9.69380737596347;8.46463938381266;0.544640564808193]
Robot Arm 1 togoal: 27.0137415824333
Step 350:
Mobile Platform 1 Pose: [-7.082112374032;7.27296771387524;10.5377167648748]
Robot Arm 1 end: [7.96257779316615;10.9045121399624;0.678306490963486]
Robot Arm 1 togoal: 2.38314158421429
Mobile Platform 2 Pose: [-4.47196343475296;9.00078397656491;8.43737956943639]
Robot Arm 2 end: [9.69619365474779;8.46070607147493;0.550052193955893]
Robot Arm 1 togoal: 27.0125980384113
Step 351:
Mobile Platform 1 Pose: [-7.12402052210376;7.29087145641181;10.528969363013]
Robot Arm 1 end: [7.93097132556796;11.0049679532563;0.557838970116253]
Robot Arm 1 togoal: 2.49369593418701
Mobile Platform 2 Pose: [-4.48248173865182;9.00589877223412;8.43422733086925]
Robot Arm 2 end: [9.69848464571103;8.45677642055801;0.555534686044978]
Robot Arm 1 togoal: 27.0113867923924
Step 352:
Mobile Platform 1 Pose: [-7.1428719308025;7.30664801844265;10.5379504189122]
Robot Arm 1 end: [7.86232658587319;11.1409941122023;0.549126129107274]
Robot Arm 1 togoal: 2.610757363385
Mobile Platform 2 Pose: [-4.49311923094839;9.01108722521273;8.43106187818462]
Robot Arm 2 end: [9.70140510002052;8.45292089019411;0.56106465465036]
Robot Arm 1 togoal: 27.0106854441373
Step 353:
Mobile Platform 1 Pose: [-7.29480717520701;7.42511738543577;10.5924778994566]
Robot Arm 1 end: [7.94624819326067;11.2138334882042;0.42768409184878]
Robot Arm 1 togoal: 2.62423853637347
Mobile Platform 2 Pose: [-4.50369292957004;9.01623558167804;8.42789207943904]
Robot Arm 2 end: [9.70445221697883;8.44873051526292;0.566829510187499]
Robot Arm 1 togoal: 27.0098414678729
Step 354:
Mobile Platform 1 Pose: [-7.62895245578744;7.63068164471461;10.6195831916511]
Robot Arm 1 end: [8.2249299878237;11.0966998776622;0.309173498569345]
Robot Arm 1 togoal: 2.41291268788605
Mobile Platform 2 Pose: [-4.51420949965211;9.02135183599998;8.42472726020287]
Robot Arm 2 end: [9.70727046327554;8.44395662031375;0.572749083132083]
Robot Arm 1 togoal: 27.0084287718862
Step 355:
Mobile Platform 1 Pose: [-8.11567437438995;7.88049365850103;10.6098900099527]
Robot Arm 1 end: [8.57287198923132;10.9527148879562;0.406628441174586]
Robot Arm 1 togoal: 2.04601661478417
Mobile Platform 2 Pose: [-4.52484000362955;9.02654811761072;8.42157434933798]
Robot Arm 2 end: [9.71006624006668;8.43860569692475;0.578544181554246]
Robot Arm 1 togoal: 27.0066124162992
Step 356:
Mobile Platform 1 Pose: [-8.70276228212673;8.12773774718556;10.5728715257697]
Robot Arm 1 end: [8.76911840850216;11.0296963256311;0.603703936221861]
Robot Arm 1 togoal: 1.84845234829826
Mobile Platform 2 Pose: [-4.53576163856665;9.03194029092265;8.41843997781422]
Robot Arm 2 end: [9.71307995312598;8.43271739417744;0.583912536802503]
Robot Arm 1 togoal: 27.004605619887
Step 357:
Mobile Platform 1 Pose: [-9.17889284050677;8.37764664425651;10.5661229581435]
Robot Arm 1 end: [9.03103254890589;11.0093386510069;0.729324738493066]
Robot Arm 1 togoal: 1.60761069708381
Mobile Platform 2 Pose: [-4.54714184568319;9.03763752966654;8.4153300303729]
Robot Arm 2 end: [9.71656621018829;8.42637000067543;0.588555731775244]
Robot Arm 1 togoal: 27.0026581438916
Step 358:
Mobile Platform 1 Pose: [-9.56638006906718;8.59810520672258;10.5756668968067]
Robot Arm 1 end: [9.31081259599701;10.8781967506217;0.860733676282849]
Robot Arm 1 togoal: 1.29698127545229
Mobile Platform 2 Pose: [-4.55912386089758;9.04373289755961;8.41224941475239]
Robot Arm 2 end: [9.72076862034533;8.41968215815346;0.592208513128694]
Robot Arm 1 togoal: 27.0010381172776
Step 359:
Mobile Platform 1 Pose: [-9.99626465202368;8.84695983978188;10.5995702774484]
Robot Arm 1 end: [9.56089441515747;10.8299361193095;0.979878695940489]
Robot Arm 1 togoal: 1.08370657582276
Mobile Platform 2 Pose: [-4.57181636781152;9.05029670326433;8.40920212282124]
Robot Arm 2 end: [9.72589381559098;8.41280924199222;0.594665202712822]
Robot Arm 1 togoal: 27.0000097622692
Step 360:
Mobile Platform 1 Pose: [-10.1063824697222;8.89919238720548;10.5882617644563]
Robot Arm 1 end: [9.66487156914709;10.6863485090405;0.845637435672884]
Robot Arm 1 togoal: 1.01955869586922
Mobile Platform 2 Pose: [-4.58528863458723;9.05737353899016;8.40619154931446]
Robot Arm 2 end: [9.73209036464419;8.40593403298213;0.595797716557811]
Robot Arm 1 togoal: 26.9998110558252
Step 361:
Mobile Platform 1 Pose: [-10.1236004477091;8.89688029734109;10.5738876099906]
Robot Arm 1 end: [9.67369975573513;10.5439233296698;0.705850757121628]
Robot Arm 1 togoal: 1.0328565854617
Mobile Platform 2 Pose: [-4.59957093821913;9.06498283946487;8.40322092679403]
Robot Arm 2 end: [9.73943727521392;8.39925293390047;0.595561432707316]
Robot Arm 1 togoal: 27.0006357149157
Step 362:
Mobile Platform 1 Pose: [-10.1984019474727;8.94595616396138;10.5793100428961]
Robot Arm 1 end: [9.71869658192351;10.5659303230746;0.64184286677736]
Robot Arm 1 togoal: 1.08274004659158
Mobile Platform 2 Pose: [-4.61465890382949;9.07312199493396;8.40029368783971]
Robot Arm 2 end: [9.74794397216561;8.39296044223062;0.59398828986617]
Robot Arm 1 togoal: 27.0026227553543
Step 363:
Mobile Platform 1 Pose: [-10.1585456592428;8.91033800740272;10.5672377910891]
Robot Arm 1 end: [9.66634073374124;10.4775498492525;0.548451881915837]
Robot Arm 1 togoal: 1.13368082213226
Mobile Platform 2 Pose: [-4.63052012110972;9.08177086192041;8.39741360787276]
Robot Arm 2 end: [9.75756031195343;8.38723508952517;0.591169753014557]
Robot Arm 1 togoal: 27.0058546986788
Step 364:
Mobile Platform 1 Pose: [-10.1480896484993;8.90009966234737;10.5643675381323]
Robot Arm 1 end: [9.62287458996655;10.5184454272417;0.408770214573893]
Robot Arm 1 togoal: 1.28376957882235
Mobile Platform 2 Pose: [-4.64710177137413;9.09089678450407;8.39458468537935]
Robot Arm 2 end: [9.76819288759928;8.38222947771215;0.587234195919285]
Robot Arm 1 togoal: 27.0103633664834
Step 365:
Mobile Platform 1 Pose: [-10.2188619629816;8.94762542338786;10.5681602671654]
Robot Arm 1 end: [9.64966683061591;10.6538918232256;0.360898803692996]
Robot Arm 1 togoal: 1.37700494978232
Mobile Platform 2 Pose: [-4.66433859155542;9.10045967051123;8.39181081980468]
Robot Arm 2 end: [9.77972335872145;8.37806567214077;0.582323309998658]
Robot Arm 1 togoal: 27.0161408777313
Step 366:
Mobile Platform 1 Pose: [-10.2258823590791;8.95202895003953;10.5685035378811]
Robot Arm 1 end: [9.63577344446729;10.707269349918;0.338088020114307]
Robot Arm 1 togoal: 1.42554258766468
Mobile Platform 2 Pose: [-4.68216084453712;9.11041694916307;8.38909540521882]
Robot Arm 2 end: [9.79202544207144;8.37483580242002;0.576570785997856]
Robot Arm 1 togoal: 27.0231531913979
Step 367:
Mobile Platform 1 Pose: [-10.2447619823797;8.96368767015038;10.5694516440174]
Robot Arm 1 end: [9.63556359968228;10.7589125556384;0.339489780251453]
Robot Arm 1 togoal: 1.45076826418018
Mobile Platform 2 Pose: [-4.70050219857449;9.12072841557415;8.38644096217557]
Robot Arm 2 end: [9.80497876627367;8.37260676999744;0.570084559649529]
Robot Arm 1 togoal: 27.0313540843259
Step 368:
Mobile Platform 1 Pose: [-10.2233094466247;8.9480636541063;10.5654164139891]
Robot Arm 1 end: [9.63236356608974;10.7065877885611;0.341784408658419]
Robot Arm 1 togoal: 1.42301520067251
Mobile Platform 2 Pose: [-4.71930759774325;9.13136108974623;8.38384890281336]
Robot Arm 2 end: [9.81847924462686;8.37142765152858;0.562932039891293]
Robot Arm 1 togoal: 27.0406983589781
Step 369:
Mobile Platform 1 Pose: [-10.2542595620791;8.96644891183344;10.5662305313317]
Robot Arm 1 end: [9.64587058915765;10.7305062420586;0.342855258437708]
Robot Arm 1 togoal: 1.43075925346685
Mobile Platform 2 Pose: [-4.73854149794642;9.14229439925781;8.38131949569708]
Robot Arm 2 end: [9.83244660067522;8.37133861560335;0.555126216897281]
Robot Arm 1 togoal: 27.0511540024206
Step 370:
Mobile Platform 1 Pose: [-10.2352232044575;8.95258214347221;10.562871846548]
Robot Arm 1 end: [9.63989813549884;10.6936914320068;0.345329651352941]
Robot Arm 1 togoal: 1.4117655344797
Mobile Platform 2 Pose: [-4.7581974234227;9.15352635430702;8.37885209357589]
Robot Arm 2 end: [9.8468302430555;8.37238067433416;0.54660907234744]
Robot Arm 1 togoal: 27.0627138332282
Step 371:
Mobile Platform 1 Pose: [-10.2580560555746;8.96580321805428;10.5631128287597]
Robot Arm 1 end: [9.65082783244081;10.7064838454194;0.346146145772153]
Robot Arm 1 togoal: 1.41468095527953
Mobile Platform 2 Pose: [-4.77831071025821;9.16508198585105;8.37644573581889]
Robot Arm 2 end: [9.86161492528906;8.37460623584496;0.537226562413744]
Robot Arm 1 togoal: 27.0754078813103
Step 372:
Mobile Platform 1 Pose: [-10.2445580366721;8.95558435784885;10.5604739713725]
Robot Arm 1 end: [9.6457319992514;10.6803359998596;0.348545679671463]
Robot Arm 1 togoal: 1.40107530929511
Mobile Platform 2 Pose: [-4.7989781067049;9.17702653080864;8.37410038671468]
Robot Arm 2 end: [9.87682775838792;8.37809107177116;0.526684968789363]
Robot Arm 1 togoal: 27.0893184772195
Step 373:
Mobile Platform 1 Pose: [-10.2604773582487;8.96442894390363;10.5602682790055]
Robot Arm 1 end: [9.65371731503198;10.6860560219001;0.349317003277713]
Robot Arm 1 togoal: 1.4012228972944
Mobile Platform 2 Pose: [-4.82039161524509;9.18948840668661;8.37181945595948]
Robot Arm 2 end: [9.89254800106916;8.38294912496873;0.514467651446706]
Robot Arm 1 togoal: 27.104600935537
Step 374:
Mobile Platform 1 Pose: [-10.2506898125357;8.95666496648261;10.5580951147009]
Robot Arm 1 end: [9.64942529526399;10.6667462223133;0.351585565345387]
Robot Arm 1 togoal: 1.39103676031525
Mobile Platform 2 Pose: [-4.84290264453296;9.20270307366829;8.36961529791662]
Robot Arm 2 end: [9.90928230852783;8.38941454006827;0.499626740426413]
Robot Arm 1 togoal: 27.1218231780702
Step 375:
Mobile Platform 1 Pose: [-10.2615156031371;8.96229127548674;10.5575820440024]
Robot Arm 1 end: [9.65494056667087;10.6681821964576;0.352394941198143]
Robot Arm 1 togoal: 1.38966587482372
Mobile Platform 2 Pose: [-4.86688556695836;9.21691670921409;8.367508899882]
Robot Arm 2 end: [9.92765048819659;8.39752946665377;0.480909349437706]
Robot Arm 1 togoal: 27.141520462522
Step 376:
Mobile Platform 1 Pose: [-10.2537144548967;8.95586354500523;10.5556863707874]
Robot Arm 1 end: [9.65101397431561;10.6530336562412;0.354550183524246]
Robot Arm 1 togoal: 1.38161115248214
Mobile Platform 2 Pose: [-4.89247077606447;9.23219620488284;8.36551766507921]
Robot Arm 2 end: [9.94710858966787;8.4065418555309;0.457628394348028]
Robot Arm 1 togoal: 27.1628180301514
Step 377:
Mobile Platform 1 Pose: [-10.2608370126602;8.95916058878109;10.5549737728325]
Robot Arm 1 end: [9.65455641803726;10.6520164540595;0.355416244480311]
Robot Arm 1 togoal: 1.37950796149025
Mobile Platform 2 Pose: [-4.91986416967643;9.24865281420721;8.36366685800308]
Robot Arm 2 end: [9.96697470778534;8.41547925866166;0.429282475959346]
Robot Arm 1 togoal: 27.1845875903946
Step 378:
Mobile Platform 1 Pose: [-10.2537542581281;8.95324750379876;10.5532348156392]
Robot Arm 1 end: [9.65056178492799;10.6393339076711;0.357505144607337]
Robot Arm 1 togoal: 1.37279830644414
Mobile Platform 2 Pose: [-4.94936035614861;9.26645524084643;8.36198864333734]
Robot Arm 2 end: [9.98672571181203;8.42003862029079;0.395688949032576]
Robot Arm 1 togoal: 27.2035540401184
Step 379:
Mobile Platform 1 Pose: [-10.2581030289868;8.95481266147576;10.5523964793832]
Robot Arm 1 end: [9.65252030324119;10.6369657244642;0.358422793960911]
Robot Arm 1 togoal: 1.37042082361715
Mobile Platform 2 Pose: [-4.98192749778958;9.28623132843862;8.36057666004982]
Robot Arm 2 end: [10.0058110144162;8.42102053263162;0.356625442326295]
Robot Arm 1 togoal: 27.2198868461779
Step 380:
Mobile Platform 1 Pose: [-10.2508459076254;8.94883596697683;10.5507381961876]
Robot Arm 1 end: [9.6480982032961;10.6257035887506;0.360503713566449]
Robot Arm 1 togoal: 1.36458473035749
Mobile Platform 2 Pose: [-5.01965404579899;9.30938593663984;8.35966814927235]
Robot Arm 2 end: [10.0234066998113;8.42301404993884;0.310554985705253]
Robot Arm 1 togoal: 27.2361788744846
Step 381:
Mobile Platform 1 Pose: [-10.2530101515983;8.94905058980691;10.5498227156845]
Robot Arm 1 end: [9.64874779391134;10.6226227744646;0.36146179549574]
Robot Arm 1 togoal: 1.36219164557309
Mobile Platform 2 Pose: [-5.0665221275968;9.33865680573555;8.35980154382986]
Robot Arm 2 end: [10.0389476948284;8.43111385892287;0.256031510728513]
Robot Arm 1 togoal: 27.2555595810377
Step 382:
Mobile Platform 1 Pose: [-10.2449154308249;8.9425800736284;10.5481942478411]
Robot Arm 1 end: [9.64359548383064;10.6121043814151;0.363602025463074]
Robot Arm 1 togoal: 1.35693257914888
Mobile Platform 2 Pose: [-5.13731345960608;9.38479232058866;8.36420490641645]
Robot Arm 2 end: [10.0631208038007;8.44393361262841;0.195187688600504]
Robot Arm 1 togoal: 27.2848910897589
Step 383:
Mobile Platform 1 Pose: [-10.2452724330153;8.94169130194762;10.5472350084199]
Robot Arm 1 end: [9.64312916246806;10.6086869589269;0.36458740333891]
Robot Arm 1 togoal: 1.35467586616314
Mobile Platform 2 Pose: [-5.23869612426881;9.45271258802394;8.37495226190549]
Robot Arm 2 end: [10.1092009379576;8.46009197188801;0.149258626036915]
Robot Arm 1 togoal: 27.3319708833952
Step 384:
Mobile Platform 1 Pose: [-10.2357736275343;8.93435770879661;10.5455967265585]
Robot Arm 1 end: [9.63695680191221;10.5984126740144;0.366870478739016]
Robot Arm 1 togoal: 1.3497825913624
Mobile Platform 2 Pose: [-5.317094589813;9.5036814214408;8.38091240650449]
Robot Arm 2 end: [10.1796497271246;8.46249905374563;0.164212097501871]
Robot Arm 1 togoal: 27.3847221609306
Step 385:
Mobile Platform 1 Pose: [-10.2345952869888;8.9325474192788;10.5446203296877]
Robot Arm 1 end: [9.63552670500861;10.5949058240803;0.367864052907375]
Robot Arm 1 togoal: 1.34776712927857
Mobile Platform 2 Pose: [-5.34056225567838;9.51676353075451;8.3785222371805]
Robot Arm 2 end: [10.195147263453;8.45961351174421;0.164929995641419]
Robot Arm 1 togoal: 27.3941641568463
Step 386:
Mobile Platform 1 Pose: [-10.2231033217543;8.9239632192149;10.5429329804699]
Robot Arm 1 end: [9.62800090831853;10.5844202484562;0.370406775590445]
Robot Arm 1 togoal: 1.34305439914942
Mobile Platform 2 Pose: [-5.35825900410555;9.52612043752175;8.37569972297274]
Robot Arm 2 end: [10.2065414738231;8.45634696114791;0.165962001846675]
Robot Arm 1 togoal: 27.4003136744827
Step 387:
Mobile Platform 1 Pose: [-10.2206548627002;8.92141320040369;10.5419670796759]
Robot Arm 1 end: [9.62576776504828;10.5810381858935;0.371370770049561]
Robot Arm 1 togoal: 1.34138082046069
Mobile Platform 2 Pose: [-5.37235138028117;9.53317496220237;8.37264161994181]
Robot Arm 2 end: [10.2154253792706;8.45287252940939;0.16722884856373]
Robot Arm 1 togoal: 27.4044635813575
Step 388:
Mobile Platform 1 Pose: [-10.2064202717792;8.91107920096723;10.5401800390043]
Robot Arm 1 end: [9.6164299915209;10.5698122780911;0.374357564309879]
Robot Arm 1 togoal: 1.33663789374437
Mobile Platform 2 Pose: [-5.38396016562471;9.53865546697345;8.36943816541476]
Robot Arm 2 end: [10.2226133254935;8.44926755591072;0.168691240290806]
Robot Arm 1 togoal: 27.4072673217085
Step 389:
Mobile Platform 1 Pose: [-10.203090743572;8.90805947703019;10.5392637083245]
Robot Arm 1 end: [9.61364102536793;10.5668358298283;0.375204195289168]
Robot Arm 1 togoal: 1.33544904109025
Mobile Platform 2 Pose: [-5.39375194335541;9.54299191603361;8.36613861365731]
Robot Arm 2 end: [10.2285853400932;8.44557317266863;0.170327296670401]
Robot Arm 1 togoal: 27.4091070865365
Step 390:
Mobile Platform 1 Pose: [-10.1849811076306;8.89521093419817;10.5372956590423]
Robot Arm 1 end: [9.60175899578047;10.5541087960916;0.378962043573013]
Robot Arm 1 togoal: 1.33036955364584
Mobile Platform 2 Pose: [-5.4021588098528;9.54646144238196;8.36277294303598]
Robot Arm 2 end: [10.2336503543783;8.44181375178351;0.172124207125256]
Robot Arm 1 togoal: 27.4102274164828
Step 391:
Mobile Platform 1 Pose: [-10.1815337807517;8.89225232121085;10.5364991665089]
Robot Arm 1 end: [9.59891104645845;10.5520561775416;0.379470608848665]
Robot Arm 1 togoal: 1.32993523251219
Mobile Platform 2 Pose: [-5.40947778428624;9.54925369833501;8.35936083002975]
Robot Arm 2 end: [10.2380194239531;8.43800483441047;0.174074469249279]
Robot Arm 1 togoal: 27.4107946047435
Step 392:
Mobile Platform 1 Pose: [-10.1575726691179;8.87553487450402;10.5341952402498]
Robot Arm 1 end: [9.58315274893972;10.5365394808093;0.384651549243869]
Robot Arm 1 togoal: 1.32398038003571
Mobile Platform 2 Pose: [-5.41592189248572;9.55150422656998;8.35591592949614]
Robot Arm 2 end: [10.2418430839756;8.43415686225097;0.176173962862158]
Robot Arm 1 togoal: 27.4109267944104
Step 393:
Mobile Platform 1 Pose: [-10.1557531398197;8.87385293963712;10.5336674050919]
Robot Arm 1 end: [9.581405979955;10.5365769546998;0.384221975083137]
Robot Arm 1 togoal: 1.32491511421156
Mobile Platform 2 Pose: [-5.42164889055475;9.55331312543339;8.35244813273016]
Robot Arm 2 end: [10.2452322190727;8.43027712033892;0.178420872115584]
Robot Arm 1 togoal: 27.410710699493
Step 394:
Mobile Platform 1 Pose: [-10.1220004672803;8.85052202854431;10.5306834621148]
Robot Arm 1 end: [9.55901685588913;10.5157981713687;0.392302366206686]
Robot Arm 1 togoal: 1.31699363533309
Mobile Platform 2 Pose: [-5.42677854858086;9.55475622867003;8.34896485347309]
Robot Arm 2 end: [10.2482705445162;8.426370829771;0.180815036497492]
Robot Arm 1 togoal: 27.4102115595001
Step 395:
Mobile Platform 1 Pose: [-10.126267494728;8.85323546014317;10.530782689252]
Robot Arm 1 end: [9.56151494790462;10.5209579884497;0.389123721489545]
Robot Arm 1 togoal: 1.3209126680047
Mobile Platform 2 Pose: [-5.43140361607285;9.55589216924486;8.34547180610187]
Robot Arm 2 end: [10.2510224770971;8.42244180101689;0.183357539187719]
Robot Arm 1 togoal: 27.4094793936826
Step 396:
Mobile Platform 1 Pose: [-10.0748373219163;8.81770897377661;10.5263470713618]
Robot Arm 1 end: [9.52675505064277;10.4923739946194;0.403496844399378]
Robot Arm 1 togoal: 1.30965882406331
Mobile Platform 2 Pose: [-5.43559708309085;9.55676703952799;8.34197349867359]
Robot Arm 2 end: [10.253538315782;8.41849284261303;0.186050435765687]
Robot Arm 1 togoal: 27.4085531037537
Step 397:
Mobile Platform 1 Pose: [-10.097571127795;8.83349141910473;10.5279868129604]
Robot Arm 1 end: [9.55766123854222;10.5086673412597;0.434569504082331]
Robot Arm 1 togoal: 1.27856844416407
Mobile Platform 2 Pose: [-5.43941716123619;9.55741757718864;8.33847355707103]
Robot Arm 2 end: [10.2558577716324;8.41452602647855;0.188896568235224]
Robot Arm 1 togoal: 27.4074632608962
Step 398:
Mobile Platform 1 Pose: [-10.0152541511179;8.77443445762797;10.5187013291418]
Robot Arm 1 end: [9.48430930887082;10.4484907132239;0.42193496685675]
Robot Arm 1 togoal: 1.29422747453587
Mobile Platform 2 Pose: [-5.44291080456648;9.55787341006088;8.33497494832027]
Robot Arm 2 end: [10.2580124422946;8.41054286974811;0.191899427897809]
Robot Arm 1 togoal: 27.4062340572681
Step 399:
Mobile Platform 1 Pose: [-10.0569155438349;8.80438665515554;10.52207167656]
Robot Arm 1 end: [9.53356197515341;10.5124911035554;0.450877251278385]
Robot Arm 1 togoal: 1.27490166663068
Mobile Platform 2 Pose: [-5.44611626218105;9.55815867876833;8.33148013473044]
Robot Arm 2 end: [10.2600275928271;8.40654445783309;0.195063059716878]
Robot Arm 1 togoal: 27.404884705284
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000996e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.35244564373649;9.99558945497965;9.8185121364336]
Robot Arm 2 end: [9.77354328092169;9.83010419669184;1.48915679394973]
Robot Arm 1 togoal: 28.0040543551092

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994556e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.15525399569139;9.86183207593441;9.81488128937989]
Robot Arm 2 end: [9.5488522970329;9.88320373041773;1.46026989446421]
Robot Arm 1 togoal: 27.8837426623871

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883228e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-2.00072698052759;9.74634950301796;9.82799498432667]
Robot Arm 2 end: [9.25003346024935;9.99270709615873;1.34409900411679]
Robot Arm 1 togoal: 27.7543405473065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.89602316040195;9.65398992310161;9.85216941362401]
Robot Arm 2 end: [9.00461968199228;9.99165256534325;1.20808611756602]
Robot Arm 1 togoal: 27.5851347041914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.926096e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.75725162131919;9.52821115037995;9.87441322491527]
Robot Arm 2 end: [8.81503227656087;10.0357372394377;1.1291758249857]
Robot Arm 1 togoal: 27.4879925145498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821036e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.59121228687408;9.44623741834322;9.86658323400743]
Robot Arm 2 end: [8.67983432896233;10.0009291896473;1.0431608533463]
Robot Arm 1 togoal: 27.3715492666535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.50566063724501;9.39835854756862;9.86191333421575]
Robot Arm 2 end: [8.63496055238022;9.83620976549232;1.06408885551784]
Robot Arm 1 togoal: 27.2203185220771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919726e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.44165597319297;9.36027693034882;9.85462985732339]
Robot Arm 2 end: [8.65245610345672;9.70032325897874;1.11249394018732]
Robot Arm 1 togoal: 27.1327059542638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954466e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.37912337800015;9.32426161276721;9.84417616432357]
Robot Arm 2 end: [8.66807760296575;9.60349675221019;1.12150465771662]
Robot Arm 1 togoal: 27.0731195622664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980737e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.31737268964297;9.28900005361461;9.82958620971139]
Robot Arm 2 end: [8.63949426897331;9.51481474231498;1.05985653218999]
Robot Arm 1 togoal: 26.990209230902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.2633973204421;9.2592650820016;9.8156702420348]
Robot Arm 2 end: [8.62992840067795;9.37871061848162;0.985466091033431]
Robot Arm 1 togoal: 26.886715195741

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.188361994608;9.22184379945482;9.80285836016888]
Robot Arm 2 end: [8.67083636854458;9.22646189943332;0.909267008531784]
Robot Arm 1 togoal: 26.8072972364649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.06064785077514;9.15857225150288;9.78214876917622]
Robot Arm 2 end: [8.73625478053678;9.08336367279668;0.827009230557865]
Robot Arm 1 togoal: 26.7526379158668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.846059949187127;9.04603668346548;9.73255673225051]
Robot Arm 2 end: [8.79954688961867;8.95291206452563;0.740302203581068]
Robot Arm 1 togoal: 26.7066532540104

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993431e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.655572684099885;8.90031580156558;9.57730077872842]
Robot Arm 2 end: [8.74956749156553;8.77152640740434;0.667067202283971]
Robot Arm 1 togoal: 26.5451631377105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.889311783381391;8.95523162877016;9.43601918646209]
Robot Arm 2 end: [8.6887846010423;8.64064648509001;0.735354898411991]
Robot Arm 1 togoal: 26.4076051445773

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994335e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-0.908191058950632;8.92607654467711;9.26500015793821]
Robot Arm 2 end: [8.7656376445162;8.43826892602644;0.805850565576776]
Robot Arm 1 togoal: 26.3178714167258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994650e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.07952328740952;9.00203983593712;9.22252354527293]
Robot Arm 2 end: [8.75476632229639;8.44539438539125;0.926431221459907]
Robot Arm 1 togoal: 26.3121140516869

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998718e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.27836396181097;9.0881869475041;9.14561914364712]
Robot Arm 2 end: [8.8151216932241;8.46123024553137;1.07907743385925]
Robot Arm 1 togoal: 26.3632532526587

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.42051469564514;9.13147866312271;8.95992281371258]
Robot Arm 2 end: [8.8848116113003;8.32372228178875;1.13207618057546]
Robot Arm 1 togoal: 26.3162719546622

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.865118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.36554850264534;9.10400517556062;8.80525245115216]
Robot Arm 2 end: [8.75204571522513;8.17156872438206;1.04084102323241]
Robot Arm 1 togoal: 26.1165786628686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.375801e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.41470967014868;9.09260634677334;8.60127252981767]
Robot Arm 2 end: [8.79829587791343;7.97721589403441;1.08431088533129]
Robot Arm 1 togoal: 26.0143598152476
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.35297361609378;9.04165680960777;8.4120624330955]
Robot Arm 2 end: [8.64058708092231;7.80753218112837;1.00057065997798]
Robot Arm 1 togoal: 25.7846957670888
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.53803842380564;9.08305803168214;8.25195813474687]
Robot Arm 2 end: [8.78852768200044;7.64161819264783;1.07311036397144]
Robot Arm 1 togoal: 25.7766574644294
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.31660103016687;8.9444080623954;8.08622178293353]
Robot Arm 2 end: [8.54706733233779;7.47559745002407;1.00341481745422]
Robot Arm 1 togoal: 25.4883916182789
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.56445437484262;9.02436854713364;7.96742010325563]
Robot Arm 2 end: [8.73015384895651;7.35803410407099;1.07238731434158]
Robot Arm 1 togoal: 25.5405807393921
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-1.32325307086284;8.86728230244664;7.80360605227895]
Robot Arm 2 end: [8.4741185439664;7.19193291999531;1.00282818559734]
Robot Arm 1 togoal: 25.2413176266886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.259573e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-1.57648561148587;8.94879279500564;7.69722655749152]
Robot Arm 2 end: [8.7664278364625;7.04329954118869;1.07399271095765]
Robot Arm 1 togoal: 25.3545398007594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.336875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-1.3725189138052;8.82307593468592;7.52779225002479]
Robot Arm 2 end: [8.54868891137637;6.85893828282216;1.00083333406822]
Robot Arm 1 togoal: 25.0708642394986

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997893e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-1.60606722494663;8.90695693300623;7.42620892644701]
Robot Arm 2 end: [8.7173433235298;6.71871102373238;0.994777935510534]
Robot Arm 1 togoal: 25.1024132116239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998881e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-1.49521241643607;8.81232876213911;7.2370302206641]
Robot Arm 2 end: [8.5188661101856;6.54015749877975;0.909030150548821]
Robot Arm 1 togoal: 24.837466037985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989662e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-1.68228879038763;8.84744143797069;7.08236914011645]
Robot Arm 2 end: [8.65396272106215;6.34550407995916;0.923920765577653]
Robot Arm 1 togoal: 24.8093194674686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.019184e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-1.50233046668043;8.71165129356768;6.91049839081732]
Robot Arm 2 end: [8.41128255865635;6.19751078920866;0.835762835020217]
Robot Arm 1 togoal: 24.5316984979329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.840166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-1.69009413448049;8.74723634157156;6.7618293015263]
Robot Arm 2 end: [8.5415513018752;5.9930347809303;0.780750585111482]
Robot Arm 1 togoal: 24.4972295449987

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024306e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-1.51104711223305;8.61562954964028;6.59294712505031]
Robot Arm 2 end: [8.29617332738092;5.85852516547013;0.69249232914164]
Robot Arm 1 togoal: 24.2266217993778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.781691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-1.66305432211949;8.63324126643312;6.4327164640605]
Robot Arm 2 end: [8.41148669816158;5.65749974502145;0.695905205312911]
Robot Arm 1 togoal: 24.1830709820351

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.018627e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-1.54537678233529;8.52959805425128;6.25126897665014]
Robot Arm 2 end: [8.19389415584386;5.52005147617276;0.606865700136265]
Robot Arm 1 togoal: 23.931682429267

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.766633e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-1.65335070976501;8.51693242428428;6.07669645310377]
Robot Arm 2 end: [8.27495393343218;5.30243345748048;0.664358457466145]
Robot Arm 1 togoal: 23.8510428644552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-1.56989047020012;8.4245193432128;5.88945452119703]
Robot Arm 2 end: [8.07191670362331;5.16524785741337;0.574228103896675]
Robot Arm 1 togoal: 23.6109146994082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-1.64864343701798;8.38760765607034;5.70710615431496]
Robot Arm 2 end: [8.14294439419431;4.92933177698209;0.656347051019185]
Robot Arm 1 togoal: 23.5116780200864

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-1.56831309805612;8.29417737859925;5.51910546791825]
Robot Arm 2 end: [7.94039418785658;4.79231497100995;0.565221722799272]
Robot Arm 1 togoal: 23.2719538915926

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [-1.64311474184028;8.25245248688641;5.3355568191665]
Robot Arm 2 end: [8.00751067199277;4.55450014985344;0.657197748233315]
Robot Arm 1 togoal: 23.1700252383154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-1.55014222577405;8.15131887562223;5.14933976078332]
Robot Arm 2 end: [7.79672354071939;4.41778082147852;0.56573460919371]
Robot Arm 1 togoal: 22.9239679062736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-1.64963994366956;8.12254970063443;4.9699792078422]
Robot Arm 2 end: [7.87723320044647;4.18532961154976;0.658309707696395]
Robot Arm 1 togoal: 22.8377599083599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-1.52998061361663;8.0070776718769;4.78791193482673]
Robot Arm 2 end: [7.65410068871428;4.05156090838476;0.566226901796997]
Robot Arm 1 togoal: 22.5837381041837

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-1.623808620635;7.97620089284261;4.61142427962668]
Robot Arm 2 end: [7.73502044763442;3.82184630853136;0.658764640637302]
Robot Arm 1 togoal: 22.5015074043853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.504483e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307232436939;-6.24962959500485;-7.10439671995214]
Robot Arm 1 end: [-6.13860042920817;-6.93244585496196;1.24046693507136]
Robot Arm 1 togoal: 23.3931794599489
Mobile Platform 2 Pose: [-1.47618247323197;7.84886422809273;4.43642548656231]
Robot Arm 2 end: [7.50040167281877;3.69530123603942;0.566861549003412]
Robot Arm 1 togoal: 22.2426553010096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.698349e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407576237151;-6.21793453138078;-7.10337755226827]
Robot Arm 1 end: [-6.10318642320225;-6.88093556700353;1.17440158062135]
Robot Arm 1 togoal: 23.3323536974315
Mobile Platform 2 Pose: [-1.57099051879246;7.82190720908161;4.26427720150708]
Robot Arm 2 end: [7.58686210305258;3.46848042353427;0.659789184686154]
Robot Arm 1 togoal: 22.1684317807852

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.983677e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998848e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859322143597;-6.18978119545286;-7.10219139620939]
Robot Arm 1 end: [-6.07853084094801;-6.82863209657295;1.10279830663555]
Robot Arm 1 togoal: 23.2786791915146
Mobile Platform 2 Pose: [-1.42546486000603;7.69912766183471;4.09409993414863]
Robot Arm 2 end: [7.35622040753127;3.35003245839452;0.567528470461971]
Robot Arm 1 togoal: 21.9173643815271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000069e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70845830412509;-6.16355485377065;-7.10107989026299]
Robot Arm 1 end: [-6.05861460659974;-6.77635574710226;1.0230563902821]
Robot Arm 1 togoal: 23.2287141933231
Mobile Platform 2 Pose: [-1.47864528370777;7.65354646504275;3.92199494195882]
Robot Arm 2 end: [7.43047968800753;3.11955574091099;0.6261783100795]
Robot Arm 1 togoal: 21.8344926858781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000200e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997358e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.74537283828259;-6.13815740100146;-7.10000570573647]
Robot Arm 1 end: [-6.04162328969002;-6.72121935862054;0.930993874016868]
Robot Arm 1 togoal: 23.1793218588493
Mobile Platform 2 Pose: [-1.32952003688575;7.53768488613604;3.76419662559937]
Robot Arm 2 end: [7.20374845694955;3.01779089386209;0.571150402263544]
Robot Arm 1 togoal: 21.5947691579381

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990853e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981784e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78014230891583;-6.11348901694205;-7.09883251983507]
Robot Arm 1 end: [-6.02899333927119;-6.65913902846831;0.819186800096386]
Robot Arm 1 togoal: 23.1289446885887
Mobile Platform 2 Pose: [-1.3412011802542;7.47897082743561;3.59807277729571]
Robot Arm 2 end: [7.2712048245695;2.79614165216018;0.594526350008661]
Robot Arm 1 togoal: 21.5149741040632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997521e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80658101976018;-6.09325369088513;-7.09790912499021]
Robot Arm 1 end: [-5.9967947189421;-6.6013202279138;0.729465982140543]
Robot Arm 1 togoal: 23.0678954543136
Mobile Platform 2 Pose: [-1.19913965039713;7.37661210997594;3.45154081173113]
Robot Arm 2 end: [7.06286476293558;2.70140371637686;0.593479632541201]
Robot Arm 1 togoal: 21.2914843145487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978811e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82830475471708;-6.07556823552737;-7.09843426427456]
Robot Arm 1 end: [-5.97835942802531;-6.56330497604686;0.646584082743045]
Robot Arm 1 togoal: 23.0307543284421
Mobile Platform 2 Pose: [-1.20215671202826;7.32281638059228;3.29390260975196]
Robot Arm 2 end: [7.14115041576394;2.49234425661076;0.589627998241272]
Robot Arm 1 togoal: 21.2307596756965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974730e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85394315545484;-6.0562768474663;-7.10074393391989]
Robot Arm 1 end: [-5.94933501511024;-6.53400010612927;0.572552365040494]
Robot Arm 1 togoal: 22.9924770272793
Mobile Platform 2 Pose: [-1.03890814062727;7.21936615815862;3.16434989870782]
Robot Arm 2 end: [6.9589931697308;2.40147585989056;0.652713299272908]
Robot Arm 1 togoal: 21.0275527494068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985816e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990698e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89414092389536;-6.02973883481752;-7.10426575541684]
Robot Arm 1 end: [-5.91488679877747;-6.51004156435453;0.511652367569284]
Robot Arm 1 togoal: 22.9539512266997
Mobile Platform 2 Pose: [-1.08790778630735;7.20183512851011;3.02157149212119]
Robot Arm 2 end: [7.07071037727799;2.21315559784936;0.618458611698758]
Robot Arm 1 togoal: 21.0091623678761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984044e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94425338368395;-5.99891027414889;-7.10840997353324]
Robot Arm 1 end: [-5.8861078335051;-6.4897206190427;0.462766739077525]
Robot Arm 1 togoal: 22.9215873321713
Mobile Platform 2 Pose: [-0.970265132181585;7.12665169366359;2.88892284950746]
Robot Arm 2 end: [6.96397789394443;2.1033069537564;0.636308726008677]
Robot Arm 1 togoal: 20.8578345916675

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981866e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99624306374689;-5.96847851552796;-7.11344393640267]
Robot Arm 1 end: [-5.86047860242607;-6.47302641801319;0.424928694251905]
Robot Arm 1 togoal: 22.8935963355227
Mobile Platform 2 Pose: [-0.976357302400493;7.10344428774096;2.7509807536267]
Robot Arm 2 end: [7.07811311891511;1.94022579702753;0.624259139449438]
Robot Arm 1 togoal: 20.8574946711594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966944e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943528e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.04630729380315;-5.9410338440552;-7.11961024062828]
Robot Arm 1 end: [-5.81089254648736;-6.46910405158097;0.388975679072317]
Robot Arm 1 togoal: 22.8581974451518
Mobile Platform 2 Pose: [-0.892923312927777;7.05817772165607;2.62694135364073]
Robot Arm 2 end: [7.10341901561066;1.83160030395441;0.58097587717242]
Robot Arm 1 togoal: 20.8181976435902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.747087e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956530e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.07860275978962;-5.92494894288765;-7.12416217355296]
Robot Arm 1 end: [-5.76197827779469;-6.47564653036722;0.374063047016422]
Robot Arm 1 togoal: 22.8298565960759
Mobile Platform 2 Pose: [-0.841874267727137;7.03365961455628;2.50646674120397]
Robot Arm 2 end: [7.1760714792151;1.72232078517657;0.591895311152181]
Robot Arm 1 togoal: 20.8157025241169

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.825928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.938745e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.1194614899037;-5.89799125523676;-7.12348325116671]
Robot Arm 1 end: [-5.76045291628935;-6.4835073647801;0.380303396546101]
Robot Arm 1 togoal: 22.8341647518026
Mobile Platform 2 Pose: [-0.811569700534738;7.02839609532188;2.39629685510582]
Robot Arm 2 end: [7.3167806148451;1.66724915301774;0.511452193731648]
Robot Arm 1 togoal: 20.9048984794093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.413106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919714e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.1657303387425;-5.86977080207499;-7.12401813122256]
Robot Arm 1 end: [-5.6422049275726;-6.55385611787067;0.346698439460875]
Robot Arm 1 togoal: 22.8054316232433
Mobile Platform 2 Pose: [-0.763047667578873;7.0258033738778;2.30119316607185]
Robot Arm 2 end: [7.43333093916988;1.6512659807022;0.433707436008454]
Robot Arm 1 togoal: 20.9965528529661

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.083231e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000179e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.19787820374625;-5.84953650487217;-7.12842919819667]
Robot Arm 1 end: [-5.65427062478406;-6.56079531364795;0.348667754071721]
Robot Arm 1 togoal: 22.818643542274
Mobile Platform 2 Pose: [-0.68881383120233;7.01865148640742;2.22906262877457]
Robot Arm 2 end: [7.47629660160606;1.65956401974336;0.33837291265707]
Robot Arm 1 togoal: 21.0419814386744

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.510976e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000698e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.22103682103307;-5.83455950761119;-7.13222510287487]
Robot Arm 1 end: [-5.67526245330932;-6.57043878756836;0.333932819917766]
Robot Arm 1 togoal: 22.8408060952915
Mobile Platform 2 Pose: [-0.608388415107358;7.01326628761608;2.16907984004114]
Robot Arm 2 end: [7.4676409159744;1.67182335613716;0.232567386358413]
Robot Arm 1 togoal: 21.0478026935399

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.046730e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000556e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.24061245094342;-5.82198012744223;-7.13546417678713]
Robot Arm 1 end: [-5.7192689367783;-6.58125553024587;0.315488547098401]
Robot Arm 1 togoal: 22.8798319209112
Mobile Platform 2 Pose: [-0.511797004359298;7.00199441071908;2.11868596709061]
Robot Arm 2 end: [7.44611379030783;1.64803687828907;0.190624642663207]
Robot Arm 1 togoal: 21.0193612675019

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000623e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.25792715790631;-5.8110002165366;-7.13805233122554]
Robot Arm 1 end: [-5.76476139173077;-6.59524991268184;0.29343233544912]
Robot Arm 1 togoal: 22.9224113865366
Mobile Platform 2 Pose: [-0.42292216735012;6.99778828479971;2.06793655607447]
Robot Arm 2 end: [7.46623172408557;1.65452593893836;0.15410417212521]
Robot Arm 1 togoal: 21.0419968088844

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000075e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.27271684195426;-5.80156887064725;-7.14025559853146]
Robot Arm 1 end: [-5.8137098478863;-6.61241666589676;0.270098069444027]
Robot Arm 1 togoal: 22.9697749722188
Mobile Platform 2 Pose: [-0.326807751563727;6.99081054522301;2.02680301341054]
Robot Arm 2 end: [7.45887870908007;1.57844899098454;0.182741608308915]
Robot Arm 1 togoal: 20.9919951966966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000786e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.28621293111237;-5.79290623850091;-7.14222087513952]
Robot Arm 1 end: [-5.88463952398903;-6.6437446067106;0.24652743909874]
Robot Arm 1 togoal: 23.0425755428722
Mobile Platform 2 Pose: [-0.227735101322048;6.9813241784872;1.98308859558771]
Robot Arm 2 end: [7.41397312553666;1.56043952584377;0.133433097288368]
Robot Arm 1 togoal: 20.947925051353

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998336e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.29765058230118;-5.78555654141824;-7.14372917672951]
Robot Arm 1 end: [-5.95169274062867;-6.68313329687763;0.222659058680537]
Robot Arm 1 togoal: 23.1185883455872
Mobile Platform 2 Pose: [-0.135579791139574;6.97468762924868;1.94089244467601]
Robot Arm 2 end: [7.38940808982863;1.44568383558592;0.181349372331223]
Robot Arm 1 togoal: 20.8612045669229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969609e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000587e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.30982214721792;-5.77772005928046;-7.14512288128483]
Robot Arm 1 end: [-6.04159564211285;-6.72925867571946;0.21634438919993]
Robot Arm 1 togoal: 23.2142846723086
Mobile Platform 2 Pose: [-0.0221759544533888;6.95738276010043;1.88888281810201]
Robot Arm 2 end: [7.25487315804803;1.35093824552478;0.14382839778419]
Robot Arm 1 togoal: 20.6995422262424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.32243130555817;-5.7696675806692;-7.14625275862641]
Robot Arm 1 end: [-6.1373964846355;-6.78017402625356;0.231033462098402]
Robot Arm 1 togoal: 23.3163852113184
Mobile Platform 2 Pose: [0.106296724157664;6.92684891806522;1.8258660302151]
Robot Arm 2 end: [7.13417088744526;1.20742361632355;0.192992111946262]
Robot Arm 1 togoal: 20.517060244261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000701e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.34136808609631;-5.75752749357807;-7.14769624185315]
Robot Arm 1 end: [-6.17743389588075;-6.77483074037059;0.288413981627137]
Robot Arm 1 togoal: 23.3371716820261
Mobile Platform 2 Pose: [0.24156074909508;6.88709770484455;1.74359719400812]
Robot Arm 2 end: [6.95519289083239;1.13427002932989;0.140745271177352]
Robot Arm 1 togoal: 20.3311494551539

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981722e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998215e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.35559303503727;-5.74822962922215;-7.14893708820634]
Robot Arm 1 end: [-6.22082741754148;-6.77536489440131;0.343867085325565]
Robot Arm 1 togoal: 23.3647972674446
Mobile Platform 2 Pose: [0.378204351613709;6.83568553503173;1.64915486265643]
Robot Arm 2 end: [6.79446307469599;1.0037802355072;0.193020966680034]
Robot Arm 1 togoal: 20.1221444620498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979534e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.700224e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.36750340071911;-5.74020519274493;-7.1501278213391]
Robot Arm 1 end: [-6.26269988590611;-6.78499553053805;0.396430719582746]
Robot Arm 1 togoal: 23.3982080212501
Mobile Platform 2 Pose: [0.427977113978627;6.80997166646275;1.52363062398595]
Robot Arm 2 end: [6.63629905324358;0.935000495893817;0.1396970558579]
Robot Arm 1 togoal: 19.9561689674558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.364216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.37774326686892;-5.73297802547255;-7.15136873626649]
Robot Arm 1 end: [-6.25300093525327;-6.82812913731192;0.326198876239775]
Robot Arm 1 togoal: 23.4259155507043
Mobile Platform 2 Pose: [0.505702998846785;6.74759148016094;1.38932639154825]
Robot Arm 2 end: [6.46815697767927;0.807276189693925;0.192975407077613]
Robot Arm 1 togoal: 19.74236718839

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.40645425614486;-5.71516983623951;-7.15347163671284]
Robot Arm 1 end: [-6.22720876341164;-6.87508770587606;0.276596550548121]
Robot Arm 1 togoal: 23.4443901451362
Mobile Platform 2 Pose: [0.523060427340864;6.67907014258928;1.234078625634]
Robot Arm 2 end: [6.41660390199803;0.674286585250436;0.136569021014513]
Robot Arm 1 togoal: 19.6306374556388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996350e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.42111963102499;-5.70456258260547;-7.15593883737659]
Robot Arm 1 end: [-6.20882598125677;-6.9277433243253;0.238181446492264]
Robot Arm 1 togoal: 23.4716884178818
Mobile Platform 2 Pose: [0.585003468055413;6.59743691342814;1.08570934089336]
Robot Arm 2 end: [6.23401122881598;0.548713183801484;0.190652435782618]
Robot Arm 1 togoal: 19.4058830011172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996946e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.43039973384916;-5.69768267117287;-7.15821777646409]
Robot Arm 1 end: [-6.20154980900936;-6.98492415201989;0.207609509451704]
Robot Arm 1 togoal: 23.5096333285813
Mobile Platform 2 Pose: [0.61739727318646;6.4869286048806;0.921207872744134]
Robot Arm 2 end: [6.13003543235148;0.410272879916503;0.133853877605258]
Robot Arm 1 togoal: 19.247753085701

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997671e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.43989832132358;-5.69154558890075;-7.16005939178202]
Robot Arm 1 end: [-6.21443496255615;-7.04493958179362;0.196926178289919]
Robot Arm 1 togoal: 23.5624921854055
Mobile Platform 2 Pose: [0.658891482467169;6.37837730929796;0.764445981537225]
Robot Arm 2 end: [5.92754155029242;0.294843596028945;0.188960802372164]
Robot Arm 1 togoal: 19.011699369783

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998548e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.44865138738544;-5.6864261150181;-7.16138997991987]
Robot Arm 1 end: [-6.23454047814611;-7.09497000683075;0.205074614179659]
Robot Arm 1 togoal: 23.612072414171
Mobile Platform 2 Pose: [0.685986808098195;6.22531092429125;0.600659204350633]
Robot Arm 2 end: [5.79253976833937;0.145251227761765;0.131838592556764]
Robot Arm 1 togoal: 18.8218013012812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.4526907628447;-5.68472943079345;-7.16203715644837]
Robot Arm 1 end: [-6.26097132171709;-7.13590290293647;0.223227811770081]
Robot Arm 1 togoal: 23.6588792903873
Mobile Platform 2 Pose: [0.676042597118339;6.10048872901566;0.444910640133556]
Robot Arm 2 end: [5.61484135741814;0.00781219161869939;0.187681888956558]
Robot Arm 1 togoal: 18.5945506174601

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999570e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.44739569483569;-5.6893525432042;-7.161551185941]
Robot Arm 1 end: [-6.30045446393246;-7.1644314266822;0.25070720206552]
Robot Arm 1 togoal: 23.7051928008294
Mobile Platform 2 Pose: [0.705636037369707;5.91501802553614;0.284448285371916]
Robot Arm 2 end: [5.44794862112691;-0.14017392077637;0.131377312204501]
Robot Arm 1 togoal: 18.378964555381

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.42847310855932;-5.70343955483167;-7.1586210543347]
Robot Arm 1 end: [-6.35829453277998;-7.17190592321601;0.285615276474009]
Robot Arm 1 togoal: 23.7485647665724
Mobile Platform 2 Pose: [0.651835967094627;5.7791847820378;0.133544651211269]
Robot Arm 2 end: [5.29604931544169;-0.306755640952935;0.188084079453884]
Robot Arm 1 togoal: 18.1577745254166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997129e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.38843035717411;-5.73286416138304;-7.1488133670976]
Robot Arm 1 end: [-6.41642642757731;-7.14343500585947;0.290983719160707]
Robot Arm 1 togoal: 23.7678219553942
Mobile Platform 2 Pose: [0.734013543331988;5.55894927720571;-0.0206380407733868]
Robot Arm 2 end: [5.0338457865566;-0.370675974892073;0.131273836940266]
Robot Arm 1 togoal: 17.9073096561929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.044112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.34826562364114;-5.76981350584138;-7.12365579440014]
Robot Arm 1 end: [-6.45487075777365;-7.09251196438433;0.263516903449687]
Robot Arm 1 togoal: 23.7591666656422
Mobile Platform 2 Pose: [0.590031639915281;5.43836668324834;-0.140612719066741]
Robot Arm 2 end: [5.05132233866229;-0.608265157892257;0.126346575455592]
Robot Arm 1 togoal: 17.7958434939558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996447e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.28225858571618;-5.82879045748979;-7.07558680717801]
Robot Arm 1 end: [-6.50744298941011;-7.00996396725963;0.242188001159318]
Robot Arm 1 togoal: 23.7375211078258
Mobile Platform 2 Pose: [0.667064552654739;5.29409779569274;-0.240963890695636]
Robot Arm 2 end: [4.91626885284391;-0.782151567287525;0.150135921184212]
Robot Arm 1 togoal: 17.5881515385037

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.423293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-4.16615382481235;-5.92033783758476;-7.00416116883743]
Robot Arm 1 end: [-6.6226319812474;-6.89156801205452;0.295159167129424]
Robot Arm 1 togoal: 23.7305636181989
Mobile Platform 2 Pose: [0.527263276848481;5.21507310981991;-0.406109162303562]
Robot Arm 2 end: [4.99055013560864;-0.982782103834871;0.122204230975794]
Robot Arm 1 togoal: 17.5494570160605

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.530000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-4.20475273367303;-5.92717383134312;-6.91372338305702]
Robot Arm 1 end: [-6.62128249625397;-6.84443830145567;0.258934520184829]
Robot Arm 1 togoal: 23.697994488715
Mobile Platform 2 Pose: [0.421399458157499;5.1340734924;-0.584336841532445]
Robot Arm 2 end: [4.93020084649719;-1.23316055602147;0.177331538323893]
Robot Arm 1 togoal: 17.3658808036951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.150755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-4.18930883900399;-5.96487613019381;-6.82320602237912]
Robot Arm 1 end: [-6.66532809331827;-6.7080026739333;0.276812742777368]
Robot Arm 1 togoal: 23.6313037249565
Mobile Platform 2 Pose: [0.363247955051282;5.04519996457286;-0.764418873761256]
Robot Arm 2 end: [4.83496552815088;-1.39538119610818;0.181598916655035]
Robot Arm 1 togoal: 17.2020249460283

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.659560e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-4.30013943195421;-5.92264172672005;-6.73420303583259]
Robot Arm 1 end: [-6.60130189418939;-6.66821605325977;0.217071008342435]
Robot Arm 1 togoal: 23.5612580690304
Mobile Platform 2 Pose: [0.26265940677341;5.00730680565;-0.864019052026821]
Robot Arm 2 end: [4.83303123228218;-1.51607690842352;0.185286876573734]
Robot Arm 1 togoal: 17.1400086379715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.650896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-4.35682535200126;-5.91311545776529;-6.63016361160964]
Robot Arm 1 end: [-6.60488479756625;-6.53817591736388;0.238424603512471]
Robot Arm 1 togoal: 23.4708001879313
Mobile Platform 2 Pose: [0.506932517677122;4.8222728483741;-0.973055670338085]
Robot Arm 2 end: [4.53751477327969;-1.56071331317103;0.144806719288647]
Robot Arm 1 togoal: 16.8657879600002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.985659e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-4.52978980705304;-5.82559045548104;-6.5580565418996]
Robot Arm 1 end: [-6.49552594210904;-6.4901514156177;0.193843679489936]
Robot Arm 1 togoal: 23.3621234847319
Mobile Platform 2 Pose: [0.455612769112441;4.68401234475624;-1.15070389317612]
Robot Arm 2 end: [4.47133359574693;-1.77100778939914;0.150670591711598]
Robot Arm 1 togoal: 16.7037005339758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.205000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-4.71022530736996;-5.73188654622203;-6.48291091340879]
Robot Arm 1 end: [-6.38917077913559;-6.37262256086485;0.179951564280619]
Robot Arm 1 togoal: 23.2048723171484
Mobile Platform 2 Pose: [0.630268136097605;4.52560038154381;-1.25604080024088]
Robot Arm 2 end: [4.23572033137737;-1.84042005568484;0.143240893342581]
Robot Arm 1 togoal: 16.4661075660533

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.416259e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-4.89581011685996;-5.6319421628865;-6.43847888885655]
Robot Arm 1 end: [-6.23288105178916;-6.35140084425548;0.105634670022532]
Robot Arm 1 togoal: 23.0841503163611
Mobile Platform 2 Pose: [0.407593010633125;4.49375136739097;-1.39080390712475]
Robot Arm 2 end: [4.2494545397105;-2.04957382688969;0.189159274591401]
Robot Arm 1 togoal: 16.3716227274883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.110793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-5.12841860933013;-5.51138641074282;-6.36992198374652]
Robot Arm 1 end: [-6.12467366443835;-6.25886342514306;0.128050563715527]
Robot Arm 1 togoal: 22.9411431448856
Mobile Platform 2 Pose: [0.709797461029388;4.29684143512268;-1.46546787844794]
Robot Arm 2 end: [3.84797221372348;-1.94680614212731;0.10694209652033]
Robot Arm 1 togoal: 16.081661154835

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.558829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-5.34859063658193;-5.40069407656317;-6.33519304299211]
Robot Arm 1 end: [-6.07614020354004;-6.14007433826127;0.251785746142885]
Robot Arm 1 togoal: 22.81567856345
Mobile Platform 2 Pose: [0.846559998034534;4.06617190558657;-1.60443910658409]
Robot Arm 2 end: [3.47750287795774;-2.00267391188865;0.158897265949938]
Robot Arm 1 togoal: 15.7307225594711

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.893362e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-5.42371201879537;-5.36484787101187;-6.32804084689583]
Robot Arm 1 end: [-6.01974509831215;-6.1222477063177;0.229001864404988]
Robot Arm 1 togoal: 22.7646296543815
Mobile Platform 2 Pose: [0.74124741273997;3.95036766182902;-1.70174348237652]
Robot Arm 2 end: [3.44639543572108;-2.17517101656652;0.149007897409522]
Robot Arm 1 togoal: 15.6178059010647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.852732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-5.49388341728162;-5.33258322239485;-6.32092791620405]
Robot Arm 1 end: [-5.96955594719267;-6.08378069844587;0.226616941219727]
Robot Arm 1 togoal: 22.7022057564694
Mobile Platform 2 Pose: [0.476367294335161;3.95727712540361;-1.78532955876495]
Robot Arm 2 end: [3.59743464881927;-2.42454963361239;0.214726645785052]
Robot Arm 1 togoal: 15.6199880716486

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.396112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-5.54953531768353;-5.30876746387448;-6.31408867952811]
Robot Arm 1 end: [-5.91976387751654;-6.032209292461;0.227510215396306]
Robot Arm 1 togoal: 22.6305928454388
Mobile Platform 2 Pose: [0.762598642296831;3.75751493191035;-1.85991850991259]
Robot Arm 2 end: [3.20408922574508;-2.33226256265535;0.198807401425192]
Robot Arm 1 togoal: 15.3261333335326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.989707e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-5.60895004837591;-5.28698623296272;-6.30474198791377]
Robot Arm 1 end: [-5.85771508787597;-5.95752008023423;0.23550272586213]
Robot Arm 1 togoal: 22.5335766837212
Mobile Platform 2 Pose: [0.982866796774696;3.49453736896646;-1.97712537851049]
Robot Arm 2 end: [2.80871670612864;-2.34418885439019;0.288103903922821]
Robot Arm 1 togoal: 14.9731326294408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.747468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-5.64206661277428;-5.29368173109212;-6.28514297091414]
Robot Arm 1 end: [-5.81669217068141;-5.89987463986674;0.219839173285227]
Robot Arm 1 togoal: 22.464789878442
Mobile Platform 2 Pose: [1.02497711605973;3.29013289996069;-2.07474027429542]
Robot Arm 2 end: [2.61902912071981;-2.31876653408612;0.198586315619775]
Robot Arm 1 togoal: 14.8320605938651

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.399941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971125e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-5.59483620692596;-5.34266556538615;-6.26779476080133]
Robot Arm 1 end: [-5.79465809211613;-5.98295855262313;0.0833981108672565]
Robot Arm 1 togoal: 22.5165025599749
Mobile Platform 2 Pose: [1.32914766942283;2.95622830466268;-2.15134972252735]
Robot Arm 2 end: [2.20073096841795;-2.17403667125043;0.283586890051943]
Robot Arm 1 togoal: 14.5476709266919

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.973716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.872910e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-5.49797814977654;-5.44296350184417;-6.22711828351639]
Robot Arm 1 end: [-5.89926784024963;-5.98430612442889;0.0659611623771736]
Robot Arm 1 togoal: 22.5920760036443
Mobile Platform 2 Pose: [1.29007098988584;2.7294862193117;-2.1793641857461]
Robot Arm 2 end: [2.03793446341426;-2.10808983871746;0.200119470909999]
Robot Arm 1 togoal: 14.4547167911149

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.566175e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.531694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-5.48584694505026;-5.44993156966088;-6.22759519116265]
Robot Arm 1 end: [-5.90616056239797;-5.93881181371932;0.0872276153876902]
Robot Arm 1 togoal: 22.5634077461898
Mobile Platform 2 Pose: [1.62496490686397;2.31381508152931;-2.18623675760326]
Robot Arm 2 end: [1.60918534815097;-1.95066461802142;0.283298703087609]
Robot Arm 1 togoal: 14.180863480045

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.461025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.023646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-5.47133833727739;-5.45347467954165;-6.22954793592667]
Robot Arm 1 end: [-5.87788801168151;-5.85056058995651;0.120087985345424]
Robot Arm 1 togoal: 22.4791048131411
Mobile Platform 2 Pose: [1.45054963132283;2.15219819833482;-2.13135562035344]
Robot Arm 2 end: [1.44064244691945;-2.08418078515125;0.239225376584288]
Robot Arm 1 togoal: 13.9710930003939

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.230802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-5.4832723647561;-5.44233565286031;-6.23282883544807]
Robot Arm 1 end: [-5.79560991950371;-5.75011493532285;0.157854524125997]
Robot Arm 1 togoal: 22.3478316351587
Mobile Platform 2 Pose: [1.49077778193133;1.90226637190755;-2.13719361134082]
Robot Arm 2 end: [1.12722503045839;-2.20710630650976;0.328256761288625]
Robot Arm 1 togoal: 13.6369705371095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-5.48822408442426;-5.43218826240243;-6.24236566327441]
Robot Arm 1 end: [-5.65822780117682;-5.75598612922521;0.0930571243726032]
Robot Arm 1 togoal: 22.259160308375
Mobile Platform 2 Pose: [1.41825568110337;1.70336447162849;-2.1756674368826]
Robot Arm 2 end: [0.937614170969046;-2.35950327789926;0.349691405212661]
Robot Arm 1 togoal: 13.3933027195549

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-5.59694904616727;-5.36136784258213;-6.26316056668739]
Robot Arm 1 end: [-5.47625552689534;-5.76922736108018;0.066680663740516]
Robot Arm 1 togoal: 22.1426751191227
Mobile Platform 2 Pose: [1.49523772082618;1.46073248623706;-2.23591459171901]
Robot Arm 2 end: [0.731172550720191;-2.29443554096571;0.261267206439197]
Robot Arm 1 togoal: 13.2710479863813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-5.7049023021751;-5.29639142360359;-6.27056384118745]
Robot Arm 1 end: [-5.3006413431504;-5.67208515984878;0.162145565671089]
Robot Arm 1 togoal: 21.9447115301836
Mobile Platform 2 Pose: [1.61138296510065;1.21307954583277;-2.27009092960288]
Robot Arm 2 end: [0.466098220097541;-2.16312925428506;0.349281146437905]
Robot Arm 1 togoal: 13.1274018796396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-5.79184641243326;-5.24513121688592;-6.27329729934607]
Robot Arm 1 end: [-5.11352516117318;-5.65311397727477;0.210510007302569]
Robot Arm 1 togoal: 21.798073401196
Mobile Platform 2 Pose: [1.62613948295586;0.963081153593934;-2.26919433635966]
Robot Arm 2 end: [0.291972606550897;-2.06782854496;0.271472647857466]
Robot Arm 1 togoal: 13.0539405058357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-5.87378106354701;-5.1970468199871;-6.27364878461394]
Robot Arm 1 end: [-4.92787970656832;-5.65333440184901;0.277596464068803]
Robot Arm 1 togoal: 21.6659761557851
Mobile Platform 2 Pose: [1.93205303395794;0.713102052531779;-2.27242683487829]
Robot Arm 2 end: [0.091415974353024;-1.920566104251;0.373805217955617]
Robot Arm 1 togoal: 12.9780578019654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-5.95506487374114;-5.1492188163357;-6.27280531489562]
Robot Arm 1 end: [-4.73654580893783;-5.6839026947053;0.363687902746021]
Robot Arm 1 togoal: 21.5520290786357
Mobile Platform 2 Pose: [2.35736305897517;0.464025545963054;-2.29389527402418]
Robot Arm 2 end: [-0.14780886094427;-1.84296857035266;0.566303690935598]
Robot Arm 1 togoal: 12.8263119027756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-6.04336450672;-5.09683659540847;-6.27110868450283]
Robot Arm 1 end: [-4.54919404627132;-5.73945526563538;0.496312891712726]
Robot Arm 1 togoal: 21.458319671514
Mobile Platform 2 Pose: [2.62513575578358;0.216292259054409;-2.26030628632952]
Robot Arm 2 end: [-0.300301046617355;-1.70546071267906;0.659432105441591]
Robot Arm 1 togoal: 12.7916316790352

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-6.14301645240127;-5.03644957539717;-6.26839486041055]
Robot Arm 1 end: [-4.39495941640654;-5.81135339309454;0.6456157115132]
Robot Arm 1 togoal: 21.4004684615511
Mobile Platform 2 Pose: [2.80519623529219;0.07730841097703;-2.23515386558764]
Robot Arm 2 end: [-0.439837129207721;-1.6841253275061;0.747485360223008]
Robot Arm 1 togoal: 12.6944401445141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-6.24301131311605;-4.97410700123586;-6.26536139480972]
Robot Arm 1 end: [-4.25889011753959;-5.93459016416269;0.799875784297043]
Robot Arm 1 togoal: 21.3950258522483
Mobile Platform 2 Pose: [2.97006247624935;-0.0960802695915332;-2.18569511017288]
Robot Arm 2 end: [-0.579225741694913;-1.63433720806623;0.838147704938664]
Robot Arm 1 togoal: 12.6175111979803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.715490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.335310e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-6.37473728570722;-4.88564526865697;-6.25879029125313]
Robot Arm 1 end: [-4.15193108448755;-6.07325961244931;0.972597193802656]
Robot Arm 1 togoal: 21.4225949321534
Mobile Platform 2 Pose: [3.09771402984679;-0.261449839189158;-2.13505378022638]
Robot Arm 2 end: [-0.694811246908767;-1.58386645981899;0.940831035279301]
Robot Arm 1 togoal: 12.5600333408968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.575403e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.098192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-6.56514122266665;-4.72900439485521;-6.2340844471126]
Robot Arm 1 end: [-3.99842108689356;-6.19828642697595;1.05682727207317]
Robot Arm 1 togoal: 21.4139144580735
Mobile Platform 2 Pose: [3.14799017747817;-0.411261360161425;-2.07217312949272]
Robot Arm 2 end: [-0.843225446165828;-1.57191260188306;1.02055858375869]
Robot Arm 1 togoal: 12.4551041357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.635145e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.065064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-6.70162613287583;-4.48236149373408;-6.1217599870107]
Robot Arm 1 end: [-3.88104086144194;-6.15948204813204;1.20587911948733]
Robot Arm 1 togoal: 21.3051978782235
Mobile Platform 2 Pose: [3.22805155530643;-0.583530975617771;-2.02961178255564]
Robot Arm 2 end: [-0.977809451932229;-1.55744744616308;1.09076620123871]
Robot Arm 1 togoal: 12.3637258555639

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.103798e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.401119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-6.35758398529402;-4.53010849357734;-6.00865584457348]
Robot Arm 1 end: [-3.76115178096575;-5.95002345404185;1.00328123115869]
Robot Arm 1 togoal: 21.0722703866212
Mobile Platform 2 Pose: [3.3627807782721;-0.744573547782126;-2.02030915579147]
Robot Arm 2 end: [-1.05150340371303;-1.58056251270377;1.18496750300258]
Robot Arm 1 togoal: 12.2912748230315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.575301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-6.56458999801313;-4.39046090462073;-5.9917922911842]
Robot Arm 1 end: [-3.76056012115634;-5.8232023619793;1.16300407849678]
Robot Arm 1 togoal: 20.9727182052284
Mobile Platform 2 Pose: [3.49218157056112;-0.87931741237687;-2.02792235156582]
Robot Arm 2 end: [-1.04357797016543;-1.70148287184162;1.32549431945639]
Robot Arm 1 togoal: 12.2115152447994

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.913639e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-6.78070091804368;-4.12853451386426;-5.88971109369041]
Robot Arm 1 end: [-3.59273427432413;-5.69362462825005;1.20981188399923]
Robot Arm 1 togoal: 20.7641305396673
Mobile Platform 2 Pose: [3.39655315945584;-0.933403467779988;-2.03918215176043]
Robot Arm 2 end: [-1.14633395117953;-1.82678485820998;1.37346695457442]
Robot Arm 1 togoal: 12.0503366864795

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.929077e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-6.59173431438832;-4.12629021852328;-5.82221504736559]
Robot Arm 1 end: [-3.47262758000507;-5.81946467348833;1.1609020996818]
Robot Arm 1 togoal: 20.7821275850927
Mobile Platform 2 Pose: [3.33282806642017;-1.02842736949394;-2.07144141965517]
Robot Arm 2 end: [-1.2070394884342;-1.95689639199912;1.42435246254634]
Robot Arm 1 togoal: 11.9170890301906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.957765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-6.86437109875959;-3.89351253773911;-5.78278977948078]
Robot Arm 1 end: [-3.33230755809915;-5.72958587357836;1.21956940402231]
Robot Arm 1 togoal: 20.6218611425403
Mobile Platform 2 Pose: [3.11212037007336;-1.04901783043176;-2.07927045177315]
Robot Arm 2 end: [-1.22483582024104;-2.04912347636516;1.44554347715994]
Robot Arm 1 togoal: 11.841690654744

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.924485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-6.69424125804461;-3.86411071050109;-5.72580391917529]
Robot Arm 1 end: [-3.19577494773512;-5.72459943489263;1.13877349818683]
Robot Arm 1 togoal: 20.5313808828775
Mobile Platform 2 Pose: [2.88317228263818;-1.17005899388866;-2.05286553678254]
Robot Arm 2 end: [-1.28771273289547;-2.05770180432584;1.46891751325766]
Robot Arm 1 togoal: 11.7892647200318

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.607811e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-6.89429001565807;-3.66209024040472;-5.70824277902841]
Robot Arm 1 end: [-3.00302733079991;-5.60713673337867;1.13294713618197]
Robot Arm 1 togoal: 20.3177760053856
Mobile Platform 2 Pose: [2.70687809521461;-1.19980122860951;-1.99960494231598]
Robot Arm 2 end: [-1.39790459096454;-1.96269602904407;1.43891218227602]
Robot Arm 1 togoal: 11.7728942472746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.669546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-6.81340095340911;-3.60485144949466;-5.67960686428664]
Robot Arm 1 end: [-2.98869865578061;-5.70200623762835;1.20418331950346]
Robot Arm 1 togoal: 20.3803745173203
Mobile Platform 2 Pose: [2.43482786617905;-1.28014033073087;-1.92070104725566]
Robot Arm 2 end: [-1.41264354545004;-1.90515793725715;1.46302036644357]
Robot Arm 1 togoal: 11.8013779083698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.967747e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-6.98181339917564;-3.40643076238348;-5.65713938382602]
Robot Arm 1 end: [-2.80151104400537;-5.57067314545579;1.1875672116582]
Robot Arm 1 togoal: 20.1602511054984
Mobile Platform 2 Pose: [2.2533400928579;-1.24079013617791;-1.8848468706681]
Robot Arm 2 end: [-1.44746348020405;-1.79871430308764;1.43180580387667]
Robot Arm 1 togoal: 11.8496803167615

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.098992e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-6.87790763642224;-3.37552200093633;-5.63264850404904]
Robot Arm 1 end: [-2.78304310065484;-5.66527287523612;1.22260060249499]
Robot Arm 1 togoal: 20.2211772003818
Mobile Platform 2 Pose: [2.52156427623124;-1.46217087224925;-1.85777892081644]
Robot Arm 2 end: [-1.70831139670499;-1.71591104979896;1.39428703168861]
Robot Arm 1 togoal: 11.7215308644048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.670181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-7.05643479602916;-3.17586541379295;-5.61351733006387]
Robot Arm 1 end: [-2.58995469734245;-5.54815093862838;1.20415031288714]
Robot Arm 1 togoal: 20.0088068263885
Mobile Platform 2 Pose: [1.93725838666218;-1.34827955117088;-1.78872444376334]
Robot Arm 2 end: [-1.55447458566666;-1.70145304661748;1.43180580543818]
Robot Arm 1 togoal: 11.8406392000596

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.726114e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-6.95640100529446;-3.15255950377631;-5.59290903255393]
Robot Arm 1 end: [-2.58395549804511;-5.62803056138991;1.24126737026609]
Robot Arm 1 togoal: 20.0666271592576
Mobile Platform 2 Pose: [2.20530618995162;-1.56951925777192;-1.76166861113223]
Robot Arm 2 end: [-1.81487295970877;-1.61844427458086;1.39428703272697]
Robot Arm 1 togoal: 11.7159223802519

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.696196e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-7.13102543969636;-2.94133732162341;-5.56982486647649]
Robot Arm 1 end: [-2.38024871722309;-5.50859801552912;1.22441175112556]
Robot Arm 1 togoal: 19.8462876956107
Mobile Platform 2 Pose: [1.62087867808972;-1.45553063874258;-1.6927538567431]
Robot Arm 2 end: [-1.66124062872185;-1.60434521439998;1.43180580655311]
Robot Arm 1 togoal: 11.8334222748617

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.250004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-7.02561844471802;-2.91818665957544;-5.54833926172167]
Robot Arm 1 end: [-2.3936614432113;-5.57756552838149;1.2758879176195]
Robot Arm 1 togoal: 19.907874615159
Mobile Platform 2 Pose: [1.88868693119501;-1.67658266709672;-1.66571360197018]
Robot Arm 2 end: [-1.92114116450854;-1.52113562218396;1.39428703211195]
Robot Arm 1 togoal: 11.7121798738504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.232430e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-7.22418248922764;-2.69771827166754;-5.52706682576426]
Robot Arm 1 end: [-2.16103731458608;-5.47733924652995;1.24311324230824]
Robot Arm 1 togoal: 19.6854281073344
Mobile Platform 2 Pose: [1.30412770139115;-1.56248867396644;-1.59694958151763]
Robot Arm 2 end: [-1.76770677446563;-1.50740505743749;1.43180580476686]
Robot Arm 1 togoal: 11.82805039785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.049425e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-7.13576090007068;-2.69134795188195;-5.51280100330097]
Robot Arm 1 end: [-2.14585931338246;-5.55339482977465;1.25400574886789]
Robot Arm 1 togoal: 19.735786660822
Mobile Platform 2 Pose: [1.5716333546864;-1.78330640799892;-1.56992839677402]
Robot Arm 2 end: [-2.02706112332171;-1.42399934302259;1.39428703261727]
Robot Arm 1 togoal: 11.7103201397668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.951696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-7.24922276061543;-2.49015666482418;-5.4853011906696]
Robot Arm 1 end: [-2.04133184585323;-5.41067425495872;1.29353894782111]
Robot Arm 1 togoal: 19.558483891771
Mobile Platform 2 Pose: [0.986932604145826;-1.66909919812487;-1.5013260639563]
Robot Arm 2 end: [-1.87381842845292;-1.41064681187166;1.43180580483022]
Robot Arm 1 togoal: 11.8245410367005

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.381841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-7.2046213220451;-2.44660845697249;-5.46238843885668]
Robot Arm 1 end: [-1.9799214130911;-5.4757272791874;1.29628255019073]
Robot Arm 1 togoal: 19.5721013132436
Mobile Platform 2 Pose: [1.25407291697338;-1.88963612071498;-1.47432749146216]
Robot Arm 2 end: [-2.13257828948743;-1.32704968252534;1.39428703229054]
Robot Arm 1 togoal: 11.710356431664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-7.33809589844397;-2.26804138821494;-5.43770309180121]
Robot Arm 1 end: [-1.81148628028967;-5.32992727755422;1.26851682490994]
Robot Arm 1 togoal: 19.3541113523271
Mobile Platform 2 Pose: [0.669221178528125;-1.77530812061941;-1.40589770854473]
Robot Arm 2 end: [-1.97952137973777;-1.31408466623922;1.43180580505776]
Robot Arm 1 togoal: 11.822908190786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.659000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-7.41708243255465;-2.15913967868283;-5.41880938006514]
Robot Arm 1 end: [-1.70339901043882;-5.25886203626168;1.264376617976]
Robot Arm 1 togoal: 19.2319597181205
Mobile Platform 2 Pose: [0.935933822527909;-1.99551794422431;-1.3789253118541]
Robot Arm 2 end: [-2.23763860274752;-1.23030082384701;1.39428703148854]
Robot Arm 1 togoal: 11.7122984199585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.865935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-7.49977525296997;-2.03162948692615;-5.39388594535125]
Robot Arm 1 end: [-1.58472570338239;-5.13849879095933;1.22167948099601]
Robot Arm 1 togoal: 19.0648789148943
Mobile Platform 2 Pose: [0.350921943818998;-1.88106183186298;-1.31067882278131]
Robot Arm 2 end: [-2.08476189121963;-1.21773272064038;1.43180580344782]
Robot Arm 1 togoal: 11.8231623726268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.777339e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-7.48794809879693;-1.94939977861208;-5.35034607743347]
Robot Arm 1 end: [-1.44691675551212;-5.14364443036242;1.2038039412715]
Robot Arm 1 togoal: 18.9858495511932
Mobile Platform 2 Pose: [0.617145150019284;-2.10089862047131;-1.2837361725868]
Robot Arm 2 end: [-2.34218860266944;-1.13376685291964;1.39428703083286]
Robot Arm 1 togoal: 11.7161521751822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.675789e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-7.58344851099197;-1.80054917773026;-5.31135642222447]
Robot Arm 1 end: [-1.27581486009833;-5.07919303863041;1.15927009546866]
Robot Arm 1 togoal: 18.8323368698451
Mobile Platform 2 Pose: [0.0319643268688022;-1.98630735250497;-1.21568357193914]
Robot Arm 2 end: [-2.18948683668437;-1.1216049477929;1.43180580473919]
Robot Arm 1 togoal: 11.8253105744353

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945715e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-7.47711890891215;-1.81368056588733;-5.28691994365587]
Robot Arm 1 end: [-1.26702328359446;-5.14943805013278;1.20117554131775]
Robot Arm 1 togoal: 18.8826262631087
Mobile Platform 2 Pose: [0.297637032369638;-2.20572558736339;-1.18877425471281]
Robot Arm 2 end: [-2.44617552132252;-1.03746172824469;1.3942870315325]
Robot Arm 1 togoal: 11.7219201798905
Step 145:
Mobile Platform 1 Pose: [-7.58723527369041;-1.57454003655131;-5.21487906741598]
Robot Arm 1 end: [-1.11730812917876;-5.02026059795468;1.25350031866324]
Robot Arm 1 togoal: 18.6888823782774
Mobile Platform 2 Pose: [-0.287721203141917;-2.09099238945443;-1.12092595912074]
Robot Arm 2 end: [-2.29364379584238;-1.02571515166257;1.43180580446849]
Robot Arm 1 togoal: 11.8293562837956
Step 146:
Mobile Platform 1 Pose: [-7.54265882290797;-1.44011012320659;-5.14392311081957]
Robot Arm 1 end: [-0.911433990428005;-5.00495926119157;1.18741236166455]
Robot Arm 1 togoal: 18.5558474576058
Mobile Platform 2 Pose: [-0.0226592482346822;-2.30994712567961;-1.09405353835272]
Robot Arm 2 end: [-2.54954743673768;-0.941399215441704;1.39428703155939]
Robot Arm 1 togoal: 11.7296013464991
Step 147:
Mobile Platform 1 Pose: [-7.73093241175495;-1.21048406890803;-5.0921688000385]
Robot Arm 1 end: [-0.754592999653236;-4.89606940161653;1.23226548695899]
Robot Arm 1 togoal: 18.3749155596696
Mobile Platform 2 Pose: [-0.608203001132089;-2.19506551401208;-1.02641975782]
Robot Arm 2 end: [-2.39718119416409;-0.930076924265758;1.43180580516325]
Robot Arm 1 togoal: 11.8352994891765
Step 148:
Mobile Platform 1 Pose: [-7.69657330601514;-1.11466314443398;-5.04389183992182]
Robot Arm 1 end: [-0.584315114059159;-4.90674662149249;1.17272083381098]
Robot Arm 1 togoal: 18.2855166634328
Mobile Platform 2 Pose: [-0.343811041270833;-2.41351248174005;-0.999587779911331]
Robot Arm 2 end: [-2.65225338253729;-0.845592863767457;1.39428703129156]
Robot Arm 1 togoal: 11.7391910516346
Step 149:
Mobile Platform 1 Pose: [-7.78823188669462;-0.932403360274413;-4.9831937202799]
Robot Arm 1 end: [-0.4792734491285;-4.78620477387423;1.22139072071689]
Robot Arm 1 togoal: 18.1255838349219
Mobile Platform 2 Pose: [-0.929548047259325;-2.29847626754253;-0.932178482582472]
Robot Arm 2 end: [-2.50004843159249;-0.834703596765656;1.43180580456922]
Robot Arm 1 togoal: 11.8431367167696
Step 150:
Mobile Platform 1 Pose: [-7.88850001132796;-0.755290245014211;-4.91994903655669]
Robot Arm 1 end: [-0.295486473043904;-4.61534958094438;1.16881079373285]
Robot Arm 1 togoal: 17.8809821410025
Mobile Platform 2 Pose: [-0.665884209309558;-2.51637193672526;-0.905390476726347]
Robot Arm 2 end: [-2.75424342823351;-0.750055981396799;1.39428703129157]
Robot Arm 1 togoal: 11.7506812066803
Step 151:
Mobile Platform 1 Pose: [-7.7407808899143;-0.740514990040494;-4.85051746918356]
Robot Arm 1 end: [-0.274546177584713;-4.62930734155567;1.20838994580679]
Robot Arm 1 togoal: 17.8796156976317
Mobile Platform 2 Pose: [-1.25182180722843;-2.40117524908649;-0.838215368932551]
Robot Arm 2 end: [-2.60219595832789;-0.739608238959;1.43180580384009]
Robot Arm 1 togoal: 11.8528610812565
Step 152:
Mobile Platform 1 Pose: [-7.96885338227744;-0.560245941015398;-4.82538858764293]
Robot Arm 1 end: [-0.0785760508211196;-4.51062287492767;1.156180597966]
Robot Arm 1 togoal: 17.6711336542583
Mobile Platform 2 Pose: [-0.988943135291829;-2.61847694335205;-0.811474790379416]
Robot Arm 2 end: [-2.85546881665635;-0.654801545425381;1.39428702979784]
Robot Arm 1 togoal: 11.7640603603506
Step 153:
Mobile Platform 1 Pose: [-7.84048278546966;-0.548101501796536;-4.77590612794345]
Robot Arm 1 end: [-0.0797496476936631;-4.55785188692269;1.21149077553219]
Robot Arm 1 togoal: 17.7095511089861
Mobile Platform 2 Pose: [-1.57508831962773;-2.50311418624131;-0.744543295114622]
Robot Arm 2 end: [-2.70357535322288;-0.644803568115838;1.4318058026552]
Robot Arm 1 togoal: 11.8644624197675
Step 154:
Mobile Platform 1 Pose: [-8.07567937843815;-0.339995374563815;-4.73765134935921]
Robot Arm 1 end: [0.138031933753345;-4.43517051597553;1.15670118043698]
Robot Arm 1 togoal: 17.4861452468774
Mobile Platform 2 Pose: [-1.31305038144799;-2.7197802018385;-0.717853573910157]
Robot Arm 2 end: [-2.95588203095381;-0.559842241490083;1.3942870288591]
Robot Arm 1 togoal: 11.7793137594874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.500000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-7.98068342787005;-0.351828706404565;-4.71150767025527]
Robot Arm 1 end: [0.163571638022268;-4.48994509930371;1.1719021435225]
Robot Arm 1 togoal: 17.5167263269868
Mobile Platform 2 Pose: [-1.90767059925656;-2.60259391983033;-0.648696551302128]
Robot Arm 2 end: [-2.85624852671992;-0.559079328971012;1.3684953765381]
Robot Arm 1 togoal: 11.8400771065408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.843003e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-8.12213008534632;-0.127896724439931;-4.62283931026478]
Robot Arm 1 end: [0.394041013501271;-4.50802986906803;1.18842729576805]
Robot Arm 1 togoal: 17.4031026927863
Mobile Platform 2 Pose: [-1.99417712325834;-2.58064691371705;-0.633611372190378]
Robot Arm 2 end: [-2.7520469283649;-0.560832471794959;1.42379740547794]
Robot Arm 1 togoal: 11.9012669787097

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.109941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.186086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-8.30634372754017;0.0499880377919958;-4.59465881890631]
Robot Arm 1 end: [0.5026892970301;-4.33058900461069;1.20720172876069]
Robot Arm 1 togoal: 17.1948585676896
Mobile Platform 2 Pose: [-1.68022845164369;-2.98580420113657;-0.584876674027962]
Robot Arm 2 end: [-2.73764457152663;-0.261265959187447;0.9315859162634]
Robot Arm 1 togoal: 12.1627363717736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.122117e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.875577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-8.43320449407954;0.198391258975555;-4.56039759595162]
Robot Arm 1 end: [0.637118389866473;-4.20000337504708;1.14794909747086]
Robot Arm 1 togoal: 17.013018981727
Mobile Platform 2 Pose: [-1.78870664619445;-2.92452197801852;-0.580369188120841]
Robot Arm 2 end: [-2.59946042788531;-0.219387295382805;0.920193963215806]
Robot Arm 1 togoal: 12.2796310502389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.451509e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.595628e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-8.32885482087644;0.186911389563102;-4.52459730698594]
Robot Arm 1 end: [0.673807189998366;-4.24459639574988;1.16001771915816]
Robot Arm 1 togoal: 17.0298768936895
Mobile Platform 2 Pose: [-1.89657933241982;-2.86310709892389;-0.575418810612809]
Robot Arm 2 end: [-2.50055395888345;-0.145362954205374;0.899672614621397]
Robot Arm 1 togoal: 12.399258443463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965341e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.454803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-8.40327968834641;0.370125181961683;-4.43166384238844]
Robot Arm 1 end: [0.804945411920953;-4.13826326347855;1.19437941573148]
Robot Arm 1 togoal: 16.8684972060717
Mobile Platform 2 Pose: [-2.00361555073627;-2.80245102865191;-0.570344945704989]
Robot Arm 2 end: [-2.43747829448378;-0.0547975731633752;0.867117968235858]
Robot Arm 1 togoal: 12.5110490195117

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.067301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.543556e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-8.49208424411591;0.538318509525128;-4.35062772735829]
Robot Arm 1 end: [0.913891325854531;-4.01500580177375;1.19536721979997]
Robot Arm 1 togoal: 16.7058012429717
Mobile Platform 2 Pose: [-2.12408227033878;-2.73501731277306;-0.564430861787463]
Robot Arm 2 end: [-2.40403445304109;0.0394366535925301;0.818706049752103]
Robot Arm 1 togoal: 12.60881428623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.588140e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.054331e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-8.4257468627961;0.553522139921007;-4.30508633756011]
Robot Arm 1 end: [0.969432367128463;-4.02104092447288;1.22048206447662]
Robot Arm 1 togoal: 16.6802593327317
Mobile Platform 2 Pose: [-2.27991915024304;-2.64940323533492;-0.554702232869382]
Robot Arm 2 end: [-2.40161009177096;0.132700570405537;0.755249315793752]
Robot Arm 1 togoal: 12.6883223539338

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.013699e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-8.68888132773006;0.749931228919285;-4.27519729970085]
Robot Arm 1 end: [1.08852690432352;-3.75401861322343;1.12211563740058]
Robot Arm 1 togoal: 16.3934892406281
Mobile Platform 2 Pose: [-2.46172390734146;-2.55261551473864;-0.53773858274464]
Robot Arm 2 end: [-2.40698992635926;0.210421128959145;0.679816356643368]
Robot Arm 1 togoal: 12.7520230367952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-8.46393989294535;0.699163203668431;-4.20589766623123]
Robot Arm 1 end: [1.01794255042852;-3.87043125047757;1.25524674416124]
Robot Arm 1 togoal: 16.5268522076836
Mobile Platform 2 Pose: [-2.65746137164123;-2.45370160766043;-0.509874568279474]
Robot Arm 2 end: [-2.40451225529681;0.281593256199337;0.625307554912778]
Robot Arm 1 togoal: 12.8142443757736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.892852e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.850260e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-8.72326453820914;0.869702356572316;-4.18912200350239]
Robot Arm 1 end: [1.1653291948034;-3.64008755609345;1.12207528936252]
Robot Arm 1 togoal: 16.256153840895
Mobile Platform 2 Pose: [-2.88027397144352;-2.349471175933;-0.466949123801361]
Robot Arm 2 end: [-2.40971250308442;0.340433262508302;0.565279987729037]
Robot Arm 1 togoal: 12.8627534021327

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003235e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-8.49504357894563;0.869830314877867;-4.06679393110456]
Robot Arm 1 end: [1.16304338244176;-3.70955242892645;1.25571504433746]
Robot Arm 1 togoal: 16.313001139269
Mobile Platform 2 Pose: [-3.13231743525176;-2.24691327536676;-0.399063285769648]
Robot Arm 2 end: [-2.42997059873301;0.390440377430393;0.476258636497567]
Robot Arm 1 togoal: 12.8979874743929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.840907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-8.78713759276233;1.06760777888523;-4.0410156122399]
Robot Arm 1 end: [1.36951719222726;-3.49628155375573;1.12306161475457]
Robot Arm 1 togoal: 16.024768461103
Mobile Platform 2 Pose: [-3.35821575861773;-2.18385806368591;-0.301495192839964]
Robot Arm 2 end: [-2.49000761340135;0.440146488613856;0.392520878941914]
Robot Arm 1 togoal: 12.9100778261461

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.468525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.536734e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-8.54232535694977;1.00732559065828;-3.96570205635816]
Robot Arm 1 end: [1.29973779345347;-3.6078251494472;1.25579741384131]
Robot Arm 1 togoal: 16.1535692703593
Mobile Platform 2 Pose: [-3.5064671386123;-2.17656707415583;-0.20151831728721]
Robot Arm 2 end: [-2.56536178017483;0.492661204114355;0.350106493140868]
Robot Arm 1 togoal: 12.9128144185695

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.854485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-8.82173919423699;1.19273618802622;-3.94424352010414]
Robot Arm 1 end: [1.49552580447038;-3.39970735529669;1.12268834289008]
Robot Arm 1 togoal: 15.8756697725233
Mobile Platform 2 Pose: [-3.58819601314365;-2.217759111147;-0.101621062336007]
Robot Arm 2 end: [-2.55275880964942;0.571103686073445;0.268477050320816]
Robot Arm 1 togoal: 12.9914759834456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.968332e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-8.57299382129424;1.14164894838201;-3.85860636267597]
Robot Arm 1 end: [1.43342966999519;-3.50036150245809;1.25593203094087]
Robot Arm 1 togoal: 15.9911271942972
Mobile Platform 2 Pose: [-3.4468537986809;-2.36368195819831;-0.0212929563134641]
Robot Arm 2 end: [-2.47604225529225;0.727125537209533;0.35053399481261]
Robot Arm 1 togoal: 13.1548908435038

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.864289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.660323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-8.85320398520162;1.32725072833513;-3.83721823592048]
Robot Arm 1 end: [1.63340501467886;-3.29488799642945;1.1230264355844]
Robot Arm 1 togoal: 15.7134446078503
Mobile Platform 2 Pose: [-3.17580054962779;-2.59982957946681;0.11319794087264]
Robot Arm 2 end: [-2.34470363513059;0.839334754825372;0.620447365044704]
Robot Arm 1 togoal: 13.3005915435921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998984e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-8.61235096749789;1.26863777702363;-3.76291651679592]
Robot Arm 1 end: [1.55971149709542;-3.40435123885523;1.25611533673697]
Robot Arm 1 togoal: 15.8425145110142
Mobile Platform 2 Pose: [-3.51438668050288;-2.41843646475934;0.215765814777229]
Robot Arm 2 end: [-2.29294322504449;0.998385046075364;0.695867292364979]
Robot Arm 1 togoal: 13.4552607458693

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.897603e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-8.90640035180223;1.46715578373524;-3.7369950897954]
Robot Arm 1 end: [1.70176122740462;-3.15610849978702;1.11544334491358]
Robot Arm 1 togoal: 15.5598339896561
Mobile Platform 2 Pose: [-3.81842476453098;-2.26429776677892;0.272036757669265]
Robot Arm 2 end: [-2.27823903325656;1.07360635136177;0.698631819411065]
Robot Arm 1 togoal: 13.5250375037146

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.022349e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-8.69122100357655;1.39108404334445;-3.68567999965374]
Robot Arm 1 end: [1.63587654808325;-3.28289169848802;1.25303729132492]
Robot Arm 1 togoal: 15.6992221783726
Mobile Platform 2 Pose: [-4.07658857971482;-2.14160317498377;0.310960490857624]
Robot Arm 2 end: [-2.29981816944842;1.09905774708554;0.714877450639082]
Robot Arm 1 togoal: 13.5326167706503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.647888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-9.02575214238849;1.6346338142584;-3.62925738803662]
Robot Arm 1 end: [1.89091548327656;-3.06003732660281;1.12513806853213]
Robot Arm 1 togoal: 15.377858542086
Mobile Platform 2 Pose: [-4.30689291092593;-2.03895953504144;0.344401545379318]
Robot Arm 2 end: [-2.33462984588813;1.09136641905996;0.741728276232726]
Robot Arm 1 togoal: 13.5049462442789

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010483e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-8.88705070659611;1.58013008181528;-3.60242134342]
Robot Arm 1 end: [1.89257852109189;-3.13360238328849;1.19475547879815]
Robot Arm 1 togoal: 15.4378829535204
Mobile Platform 2 Pose: [-4.52429363624496;-1.95089633127308;0.378949661296752]
Robot Arm 2 end: [-2.37115174151154;1.0582568601744;0.78062183928056]
Robot Arm 1 togoal: 13.4548329739728

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.516051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-8.60580157472851;1.51449632929806;-3.51071049468843]
Robot Arm 1 end: [1.76567834491168;-3.20645418941033;1.30568870891576]
Robot Arm 1 togoal: 15.5647307823977
Mobile Platform 2 Pose: [-4.73162438719786;-1.88463149073745;0.42321657366853]
Robot Arm 2 end: [-2.40922245271728;1.01050775367233;0.832365757035373]
Robot Arm 1 togoal: 13.3912434779819

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.890467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-8.79601530276135;1.62720069829002;-3.50492966763656]
Robot Arm 1 end: [1.92658876654793;-3.02903940037495;1.19792588932196]
Robot Arm 1 togoal: 15.3310212811131
Mobile Platform 2 Pose: [-4.93133043366376;-1.86356419471441;0.488058603220936]
Robot Arm 2 end: [-2.45187266871114;0.951438776626974;0.934672658603577]
Robot Arm 1 togoal: 13.3136027144732

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.996512e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-8.87679349041603;1.84907132341947;-3.33066536831103]
Robot Arm 1 end: [2.13635163231321;-2.82707793977684;1.13566111065671]
Robot Arm 1 togoal: 15.0505607929862
Mobile Platform 2 Pose: [-4.93549143988943;-1.99959572523261;0.563307408480776]
Robot Arm 2 end: [-2.49616428804609;1.11974673825871;0.943015618206198]
Robot Arm 1 togoal: 13.4272254714296

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-8.82588742634299;1.83317682637796;-3.31662501028202]
Robot Arm 1 end: [2.19820990703721;-2.91884169195469;1.20840429612089]
Robot Arm 1 togoal: 15.0951023643605
Mobile Platform 2 Pose: [-5.20191372939186;-1.96855210199301;0.635260457628297]
Robot Arm 2 end: [-2.46483634069307;1.02055624969228;1.11840985522785]
Robot Arm 1 togoal: 13.3564003477673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007656e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-8.66780293937591;1.78792267043868;-3.27852237878271]
Robot Arm 1 end: [2.21206201702447;-3.01974083117301;1.27493009205515]
Robot Arm 1 togoal: 15.1732059808461
Mobile Platform 2 Pose: [-5.05651130359932;-2.11646869069494;0.6680018926139]
Robot Arm 2 end: [-2.5773892580061;1.15881682404547;1.0878722910336]
Robot Arm 1 togoal: 13.4090246756443

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.584443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-8.80557146522724;2.0457483184266;-3.13462115012817]
Robot Arm 1 end: [2.31368607195418;-2.83763482620849;1.32746247668986]
Robot Arm 1 togoal: 14.9640150529355
Mobile Platform 2 Pose: [-5.39327116587757;-2.05859783949326;0.741594024807846]
Robot Arm 2 end: [-2.47176726886696;1.06480626106795;1.23425079986764]
Robot Arm 1 togoal: 13.3860543384841

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.564977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-8.82138562511141;2.12313361746321;-3.08632580061221]
Robot Arm 1 end: [2.45058457452629;-2.74475093090856;1.26689382790147]
Robot Arm 1 togoal: 14.8150909380933
Mobile Platform 2 Pose: [-5.25826580613171;-2.16152789021436;0.7530632004514]
Robot Arm 2 end: [-2.60975784206356;1.21943749604304;1.13014266815105]
Robot Arm 1 togoal: 13.4403953191272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.603316e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-8.86114571242861;2.28659519527586;-2.97716461520345]
Robot Arm 1 end: [2.56793214450355;-2.70626303058787;1.2240110409216]
Robot Arm 1 togoal: 14.72321144496
Mobile Platform 2 Pose: [-5.603012997372;-2.08599183502543;0.819008928132136]
Robot Arm 2 end: [-2.48320177022671;1.13503219557963;1.26257291495251]
Robot Arm 1 togoal: 13.4371865426511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-8.92011989698676;2.3289582771921;-2.96959159075299]
Robot Arm 1 end: [2.61104345520575;-2.65348431000874;1.27505954568194]
Robot Arm 1 togoal: 14.6549592531527
Mobile Platform 2 Pose: [-5.4695836010003;-2.18176721555622;0.827294777401431]
Robot Arm 2 end: [-2.64094867280736;1.30293640806456;1.12636918233381]
Robot Arm 1 togoal: 13.4932479916164

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-9.05863188997183;2.42338286698543;-2.95758979047458]
Robot Arm 1 end: [2.6521288981957;-2.58863498936009;1.31762585944104]
Robot Arm 1 togoal: 14.5775958808413
Mobile Platform 2 Pose: [-5.90446012961471;-2.04925807651157;0.886269761828488]
Robot Arm 2 end: [-2.53427676662977;1.28903263300719;1.13959730736962]
Robot Arm 1 togoal: 13.5397470138969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.172409e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.664516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-9.37668767499493;2.60383186566474;-2.96114020713613]
Robot Arm 1 end: [2.82347035000134;-2.42182417958327;1.22448659959939]
Robot Arm 1 togoal: 14.3489446985626
Mobile Platform 2 Pose: [-5.7532417755173;-2.19933006325069;0.917809222804627]
Robot Arm 2 end: [-2.66844010256299;1.41898481534814;1.09071760197138]
Robot Arm 1 togoal: 13.5768231808811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.187253e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.757869e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-9.62961499233489;2.74707869816528;-2.9647747654932]
Robot Arm 1 end: [3.07307526472148;-2.34625659297069;1.11485968912147]
Robot Arm 1 togoal: 14.1625311332334
Mobile Platform 2 Pose: [-6.05924896611214;-2.11385664470444;0.967558180083439]
Robot Arm 2 end: [-2.63592122535374;1.33974735319574;1.15332469785056]
Robot Arm 1 togoal: 13.5260752386387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.079269e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.081537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-9.72659923319599;2.80321961638552;-2.96557982574117]
Robot Arm 1 end: [3.23832893386342;-2.38154412842766;1.07988211069244]
Robot Arm 1 togoal: 14.1144399677457
Mobile Platform 2 Pose: [-5.97893674075728;-2.30859674130204;1.03789635417904]
Robot Arm 2 end: [-2.80108131212834;1.49426841669646;1.08245364768871]
Robot Arm 1 togoal: 13.5696337317106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.861348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-9.80888571309041;2.85245866190695;-2.96496399286753]
Robot Arm 1 end: [3.38211414591629;-2.44741859075597;1.06157382118817]
Robot Arm 1 togoal: 14.1048117734678
Mobile Platform 2 Pose: [-6.28031039875521;-2.22127478448605;1.08205334317014]
Robot Arm 2 end: [-2.86127628773106;1.5337299430983;0.887170134344946]
Robot Arm 1 togoal: 13.5790295286162

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-9.88732376555442;2.90049640011837;-2.96340723092882]
Robot Arm 1 end: [3.51139303181535;-2.53618265802989;1.04698072331303]
Robot Arm 1 togoal: 14.1238305815998
Mobile Platform 2 Pose: [-6.38856049147377;-2.28326226762536;1.14773502912191]
Robot Arm 2 end: [-2.99266753214293;1.46909341992864;0.864264520871757]
Robot Arm 1 togoal: 13.4563781783837

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983756e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-9.97048006232133;2.95219220911852;-2.96090987740887]
Robot Arm 1 end: [3.62855556507051;-2.64504073526752;1.02829488974572]
Robot Arm 1 togoal: 14.168102120949
Mobile Platform 2 Pose: [-6.35137215172995;-2.43621848207226;1.19976172045464]
Robot Arm 2 end: [-3.05283515496313;1.62417951870994;0.944913382413931]
Robot Arm 1 togoal: 13.5542068989031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004162e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-10.0620256834186;3.00979228156185;-2.9572569496221]
Robot Arm 1 end: [3.71431387573769;-2.77144828515804;1.02855960111704]
Robot Arm 1 togoal: 14.2429715614285
Mobile Platform 2 Pose: [-6.58991718845731;-2.42468144702698;1.25435982047229]
Robot Arm 2 end: [-3.11286631676475;1.57350077321435;0.913229932856099]
Robot Arm 1 togoal: 13.4813914331149

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-10.1770313893272;3.08403624683504;-2.94992651013271]
Robot Arm 1 end: [3.76286315379045;-2.90022250747133;1.06500413951128]
Robot Arm 1 togoal: 14.3361622830309
Mobile Platform 2 Pose: [-6.53347413140359;-2.58397820274134;1.30298399745919]
Robot Arm 2 end: [-3.17928804535675;1.71859099087708;0.993167291773188]
Robot Arm 1 togoal: 13.5693070474344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008444e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-10.3774264569384;3.22928723255769;-2.9135789244755]
Robot Arm 1 end: [3.8447020552016;-2.98407873708717;1.11068232289626]
Robot Arm 1 togoal: 14.3750601402659
Mobile Platform 2 Pose: [-6.77508126166624;-2.58218986424094;1.36064786068598]
Robot Arm 2 end: [-3.22483829604172;1.66416544215424;0.999148775154649]
Robot Arm 1 togoal: 13.4991814669483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962088e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-10.2434540627609;3.24252148642213;-2.72183504813685]
Robot Arm 1 end: [3.86878304838567;-2.62094890738728;1.12338057451896]
Robot Arm 1 togoal: 14.0370322304312
Mobile Platform 2 Pose: [-6.67097410064426;-2.75236340194283;1.40178203602761]
Robot Arm 2 end: [-3.33322116169887;1.80546192157875;1.01874861676563]
Robot Arm 1 togoal: 13.5671341009303

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007885e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-10.4695650845697;3.39670250585149;-2.67212279933166]
Robot Arm 1 end: [3.95271323932692;-2.73615450406141;1.18015638794291]
Robot Arm 1 togoal: 14.103030989295
Mobile Platform 2 Pose: [-6.91533694495072;-2.74566325105107;1.45844923453623]
Robot Arm 2 end: [-3.38581131750581;1.76232173234743;0.999682056692166]
Robot Arm 1 togoal: 13.5044983935317

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970551e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-10.3236201239543;3.37910375796554;-2.44538185084184]
Robot Arm 1 end: [3.96871459531471;-2.34544782398374;1.16793495995934]
Robot Arm 1 togoal: 13.7444949172377
Mobile Platform 2 Pose: [-6.82464281964796;-2.91078907093806;1.50130154108261]
Robot Arm 2 end: [-3.48560192718103;1.90313282155366;1.02352065615502]
Robot Arm 1 togoal: 13.5782708367541

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006565e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-10.5630064298553;3.54593948534774;-2.3729966696557]
Robot Arm 1 end: [4.07479844743119;-2.4378410887109;1.20645496427522]
Robot Arm 1 togoal: 13.7806691770062
Mobile Platform 2 Pose: [-7.06420611186238;-2.90536939148695;1.55757168801612]
Robot Arm 2 end: [-3.54417177296327;1.8630301473723;0.994780894126566]
Robot Arm 1 togoal: 13.5161425311312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-10.3239169000831;3.43898688084573;-2.20726173671251]
Robot Arm 1 end: [4.10509378171797;-2.1078552070666;1.06717651920424]
Robot Arm 1 togoal: 13.4742729966908
Mobile Platform 2 Pose: [-6.98458146004908;-3.06309184487257;1.60079965900762]
Robot Arm 2 end: [-3.61689334823192;1.99753229814432;1.05033162584392]
Robot Arm 1 togoal: 13.5980278162421

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005259e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-10.5511224365223;3.59273474962452;-2.16378429132594]
Robot Arm 1 end: [4.13899292334996;-2.22396231270921;1.1905389394632]
Robot Arm 1 togoal: 13.5604521712076
Mobile Platform 2 Pose: [-7.21727749094295;-3.07077804580568;1.66086094554263]
Robot Arm 2 end: [-3.68392176636644;1.96189984890955;1.02275173137053]
Robot Arm 1 togoal: 13.5361790615341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-10.5479127669772;3.66601114638252;-1.92476427191717]
Robot Arm 1 end: [4.13976418890982;-1.99404591398997;1.26878842074289]
Robot Arm 1 togoal: 13.3515209556513
Mobile Platform 2 Pose: [-7.13845900040285;-3.22032915010482;1.70257563655701]
Robot Arm 2 end: [-3.76373217115433;2.10298001375769;1.04547165182275]
Robot Arm 1 togoal: 13.6234829928486

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003619e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-10.6534661694052;3.78105753353205;-1.75203984342344]
Robot Arm 1 end: [4.29588859921255;-1.88899396462912;1.20735025731142]
Robot Arm 1 togoal: 13.1902782580456
Mobile Platform 2 Pose: [-7.35733549732556;-3.22768597139709;1.76161239136619]
Robot Arm 2 end: [-3.84829696536688;2.07348650179803;0.991097737256123]
Robot Arm 1 togoal: 13.5607272276775

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960219e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994100e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-10.61451995594;3.84413781075899;-1.51487778325147]
Robot Arm 1 end: [4.31204199968154;-1.59426995200954;1.28217375086823]
Robot Arm 1 togoal: 12.9165397808675
Mobile Platform 2 Pose: [-7.32002762470375;-3.35478678267016;1.80648676610009]
Robot Arm 2 end: [-3.90520148592922;2.22073449149832;1.01034380875793]
Robot Arm 1 togoal: 13.6657853848387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001947e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-10.7944550745004;4.00170258256723;-1.39524909033348]
Robot Arm 1 end: [4.46154088209675;-1.64954175840494;1.21374229486904]
Robot Arm 1 togoal: 12.9027423394745
Mobile Platform 2 Pose: [-7.51464117227158;-3.36579122013391;1.86311811865504]
Robot Arm 2 end: [-4.00616622819629;2.18728749924871;0.935241751735918]
Robot Arm 1 togoal: 13.5940844707824

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.892013e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-10.6173773118471;3.95658305756889;-1.25312505689646]
Robot Arm 1 end: [4.52310402464486;-1.34762427672242;1.19412059254717]
Robot Arm 1 togoal: 12.6044363776227
Mobile Platform 2 Pose: [-7.50721434057878;-3.47155570303572;1.90735167199267]
Robot Arm 2 end: [-4.06913743457074;2.3328602389841;0.906567040471363]
Robot Arm 1 togoal: 13.6986167296585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000646e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-10.835816106845;4.13036368476734;-1.19045089661818]
Robot Arm 1 end: [4.64634327510314;-1.45233122212659;1.16020542162865]
Robot Arm 1 togoal: 12.6470432701525
Mobile Platform 2 Pose: [-7.66916387099599;-3.48617186684754;1.95889374154445]
Robot Arm 2 end: [-4.17403546514464;2.27943349338363;0.829082788807403]
Robot Arm 1 togoal: 13.6090080133333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001823e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-10.6518659863369;4.10271715412377;-1.04489357305214]
Robot Arm 1 end: [4.75944322696459;-1.14835638622703;1.08140055285112]
Robot Arm 1 togoal: 12.3264971942426
Mobile Platform 2 Pose: [-7.65772173385648;-3.59681724510796;2.00064623308286]
Robot Arm 2 end: [-4.23573122466161;2.43299409159911;0.79078771953687]
Robot Arm 1 togoal: 13.723678317806

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000147e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000260e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-10.8686423352844;4.26738655603863;-1.00326346251731]
Robot Arm 1 end: [4.78680716883977;-1.23627036641318;1.1706183640809]
Robot Arm 1 togoal: 12.3916874772076
Mobile Platform 2 Pose: [-7.81272993061827;-3.61709698396652;2.05072197961531]
Robot Arm 2 end: [-4.33887019824364;2.3655165099404;0.710019048549227]
Robot Arm 1 togoal: 13.6239524108666

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000081e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.981120e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-10.8432919975264;4.36693957438303;-0.810814821547802]
Robot Arm 1 end: [4.85832825840675;-0.963468867674547;1.24787041773196]
Robot Arm 1 togoal: 12.1123506256828
Mobile Platform 2 Pose: [-7.77469790933647;-3.7446771625787;2.0892195514096]
Robot Arm 2 end: [-4.39631632125704;2.53096638206084;0.734635909908788]
Robot Arm 1 togoal: 13.7493548111151

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001638e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-11.027392885745;4.54392200611135;-0.693949923258786]
Robot Arm 1 end: [4.92183238556559;-1.00470507366497;1.24047713134156]
Robot Arm 1 togoal: 12.1231189534079
Mobile Platform 2 Pose: [-7.94338579077685;-3.75814092658622;2.13686132888176]
Robot Arm 2 end: [-4.4884925168818;2.45793058637426;0.65247105479741]
Robot Arm 1 togoal: 13.6503147128571

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006292e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-10.9118967457812;4.5364355855545;-0.56310730565721]
Robot Arm 1 end: [4.95416152546217;-0.71707280570968;1.30393043561124]
Robot Arm 1 togoal: 11.8474999316224
Mobile Platform 2 Pose: [-7.90008178652434;-3.8912840233304;2.17425847231355]
Robot Arm 2 end: [-4.55072099625065;2.62519951132555;0.68680983463871]
Robot Arm 1 togoal: 13.7762904148201

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.947715e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-11.1024582674412;4.70205063378107;-0.472768215448739]
Robot Arm 1 end: [5.00132697272417;-0.850040841725455;1.24590999843282]
Robot Arm 1 togoal: 11.9493009339328
Mobile Platform 2 Pose: [-8.07340024619659;-3.90058855063238;2.21941729095521]
Robot Arm 2 end: [-4.63427449025821;2.5455712655152;0.613931109693048]
Robot Arm 1 togoal: 13.6749823618086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010717e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-11.0066770150601;4.68552185301306;-0.383867362763673]
Robot Arm 1 end: [5.04765712909755;-0.600764578119018;1.31885907221061]
Robot Arm 1 togoal: 11.7022549325491
Mobile Platform 2 Pose: [-8.0321949786929;-4.03298536917936;2.25566339116069]
Robot Arm 2 end: [-4.69660992245475;2.71360405714999;0.645444489153421]
Robot Arm 1 togoal: 13.8031978863291

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919581e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-11.1587952144267;4.83108816867709;-0.296649146064779]
Robot Arm 1 end: [5.17220813602669;-0.668519038438957;1.24300131009032]
Robot Arm 1 togoal: 11.7133324048698
Mobile Platform 2 Pose: [-8.20536863809135;-4.03935625056173;2.29881995301038]
Robot Arm 2 end: [-4.77504670659327;2.62940863010774;0.597855110886758]
Robot Arm 1 togoal: 13.6986968632355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990560e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-11.0705969126576;4.82569885289722;-0.220271986504159]
Robot Arm 1 end: [5.2338783159188;-0.437492056217254;1.3027263842754]
Robot Arm 1 togoal: 11.4762711584551
Mobile Platform 2 Pose: [-8.17334581708325;-4.16647893646786;2.33424737243143]
Robot Arm 2 end: [-4.83207914285855;2.79751616362001;0.627096985042154]
Robot Arm 1 togoal: 13.8305057226388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974644e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-11.2132640474927;4.9644794686401;-0.15686256623197]
Robot Arm 1 end: [5.324526753596;-0.535258882800902;1.24001328082539]
Robot Arm 1 togoal: 11.5295569447324
Mobile Platform 2 Pose: [-8.34341310982289;-4.17023817871973;2.37543321154743]
Robot Arm 2 end: [-4.9076432272974;2.71033261307952;0.608189376918518]
Robot Arm 1 togoal: 13.7228960452673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-11.1718917116074;5.00352073642081;-0.0913888834867178]
Robot Arm 1 end: [5.41680973111646;-0.314503529539581;1.30837784764814]
Robot Arm 1 togoal: 11.2889248505491
Mobile Platform 2 Pose: [-8.32163526028277;-4.28931912213032;2.40983120423995]
Robot Arm 2 end: [-4.95541956972642;2.87720301704424;0.63752704893028]
Robot Arm 1 togoal: 13.8582348701032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-11.3120156836822;5.13624913443237;-0.0426265385863928]
Robot Arm 1 end: [5.48464191715588;-0.407086409506687;1.26974625211268]
Robot Arm 1 togoal: 11.3472038228399
Mobile Platform 2 Pose: [-8.48642689149752;-4.29166408536333;2.449266208054]
Robot Arm 2 end: [-5.02899892145401;2.78756156225816;0.646989122016859]
Robot Arm 1 togoal: 13.7475989702304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000399e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-11.3321240067884;5.21746275926265;0.019960741430951]
Robot Arm 1 end: [5.57817691519166;-0.226795832639416;1.33580407840894]
Robot Arm 1 togoal: 11.143346441945
Mobile Platform 2 Pose: [-8.470748725155;-4.4036799594807;2.48236740131892]
Robot Arm 2 end: [-5.067600781435;2.9514497529307;0.679036016938456]
Robot Arm 1 togoal: 13.884434309786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-11.4644150331851;5.34631879764982;0.0749482078483416]
Robot Arm 1 end: [5.53456540416892;-0.14629237680444;1.407776623365]
Robot Arm 1 togoal: 11.0860351189351
Mobile Platform 2 Pose: [-8.63046374912011;-4.40642809145026;2.52061514713427]
Robot Arm 2 end: [-5.13787859493714;2.85932924886954;0.713163959892878]
Robot Arm 1 togoal: 13.771534858399

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965779e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-11.5815043919782;5.46293217427944;0.153519423400092]
Robot Arm 1 end: [5.54580054218006;0.00915275413285341;1.44855473554267]
Robot Arm 1 togoal: 10.9390205144993
Mobile Platform 2 Pose: [-8.61303692395583;-4.51621442771724;2.55235914930246]
Robot Arm 2 end: [-5.17187786985978;3.0177173964609;0.749423398598696]
Robot Arm 1 togoal: 13.9056484915124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981182e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.960071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-11.6962449266449;5.56159393038146;0.264537579671751]
Robot Arm 1 end: [5.51711715131852;0.266773842067153;1.46866645879943]
Robot Arm 1 togoal: 10.7160939187749
Mobile Platform 2 Pose: [-8.76818470876768;-4.52323360133658;2.59052299860545]
Robot Arm 2 end: [-5.23891226165916;2.92466624511248;0.792798985673574]
Robot Arm 1 togoal: 13.7929413366764

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999521e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024153e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-11.7631271928496;5.60475201771952;0.418393303707273]
Robot Arm 1 end: [5.5740686384207;0.561574236852467;1.4724869193298]
Robot Arm 1 togoal: 10.4247351440147
Mobile Platform 2 Pose: [-8.7385114058049;-4.63785142471809;2.62093356722492]
Robot Arm 2 end: [-5.28021986126595;3.07874461283792;0.809111877650007]
Robot Arm 1 togoal: 13.922523833866

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.933361e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.896300e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-11.9036357919237;5.69494603463671;0.577097679826371]
Robot Arm 1 end: [5.61088491634779;0.877104787476918;1.42697306007567]
Robot Arm 1 togoal: 10.1242475938537
Mobile Platform 2 Pose: [-8.90129377383489;-4.64582611600378;2.65973733565613]
Robot Arm 2 end: [-5.348319304846;2.99902119076211;0.807986422579269]
Robot Arm 1 togoal: 13.8246545551467

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.873203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000160e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-12.0702834768057;5.80017684676033;0.628222356876831]
Robot Arm 1 end: [5.6100840308317;1.00401240772412;1.35132401631314]
Robot Arm 1 togoal: 10.0113907583403
Mobile Platform 2 Pose: [-8.87420455010453;-4.75666544456622;2.68943584994395]
Robot Arm 2 end: [-5.38803977066895;3.15629622531763;0.803183293535888]
Robot Arm 1 togoal: 13.9597123304643

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.925363e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974748e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-12.235039085326;5.89604011583583;0.644141502259192]
Robot Arm 1 end: [5.64092390202495;1.08784279687026;1.28304276304548]
Robot Arm 1 togoal: 9.92394649763157
Mobile Platform 2 Pose: [-9.03124638195063;-4.76774417875914;2.72845290611724]
Robot Arm 2 end: [-5.44012973271859;3.12517026809856;0.785928290734669]
Robot Arm 1 togoal: 13.914123825767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988628e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-12.3990717769529;5.9881337763837;0.646497745904779]
Robot Arm 1 end: [5.62493818879841;1.11102151615931;1.2119418754925]
Robot Arm 1 togoal: 9.91214513928954
Mobile Platform 2 Pose: [-9.03014330308867;-4.86089439252166;2.76065215737855]
Robot Arm 2 end: [-5.45277227653003;3.28747704779955;0.754259158585017]
Robot Arm 1 togoal: 14.0649286447812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-12.5259663366926;6.05862881475607;0.649132567728899]
Robot Arm 1 end: [5.68795509353551;1.21753999223716;1.10414342304301]
Robot Arm 1 togoal: 9.79280881407741
Mobile Platform 2 Pose: [-9.18167910173793;-4.86357850017606;2.80005191373957]
Robot Arm 2 end: [-5.46706457393878;3.30494934592883;0.745894485328403]
Robot Arm 1 togoal: 14.077285577605

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-12.63352763537;6.11772284759289;0.650387416500306]
Robot Arm 1 end: [5.731715150219;1.28231380325487;0.997768105261222]
Robot Arm 1 togoal: 9.72060079416198
Mobile Platform 2 Pose: [-9.24671636652186;-4.91170561436142;2.83817237347979]
Robot Arm 2 end: [-5.40685627625201;3.46085720856848;0.705079015527749]
Robot Arm 1 togoal: 14.2463108596358

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994697e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-12.7257595680059;6.1679635102822;0.652110659348711]
Robot Arm 1 end: [5.78011715736575;1.32305947407267;0.913733560703237]
Robot Arm 1 togoal: 9.66775468370682
Mobile Platform 2 Pose: [-9.38922004434649;-4.89668725884216;2.87679178049749]
Robot Arm 2 end: [-5.38846407356432;3.49868159879589;0.69357362657536]
Robot Arm 1 togoal: 14.2886424446102

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999564e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-12.8081305075077;6.21252172860539;0.654243789826743]
Robot Arm 1 end: [5.82451257011113;1.34640273091805;0.851512459064895]
Robot Arm 1 togoal: 9.63159667656874
Mobile Platform 2 Pose: [-9.5114755401613;-4.8907937905344;2.91561265607142]
Robot Arm 2 end: [-5.3389216913177;3.5696624848346;0.65089754329762]
Robot Arm 1 togoal: 14.3742293441129

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000223e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-12.8838651900555;6.25320782133971;0.656821523553023]
Robot Arm 1 end: [5.86265111816349;1.35851311788575;0.809416999598014]
Robot Arm 1 togoal: 9.60725255576472
Mobile Platform 2 Pose: [-9.64397544386549;-4.86702985423872;2.95078745424353]
Robot Arm 2 end: [-5.28722309908057;3.62335773427882;0.604468979043445]
Robot Arm 1 togoal: 14.4445896856349

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000490e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-12.9553148644402;6.29128020942911;0.659942874051765]
Robot Arm 1 end: [5.89422305703071;1.36348734537053;0.785086991960652]
Robot Arm 1 togoal: 9.59105432652713
Mobile Platform 2 Pose: [-9.78152362714564;-4.83591763584792;2.98528745984009]
Robot Arm 2 end: [-5.23427717263505;3.66769582322528;0.55552162850083]
Robot Arm 1 togoal: 14.5069008292367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000592e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-13.0244882961663;6.32773894610387;0.66379485641879]
Robot Arm 1 end: [5.91935288771868;1.36343525185881;0.776066688098835]
Robot Arm 1 togoal: 9.58106764108182
Mobile Platform 2 Pose: [-9.90668534863823;-4.80664120332879;3.0183547868888]
Robot Arm 2 end: [-5.18834969980562;3.70462585963273;0.506152376660577]
Robot Arm 1 togoal: 14.5601739090738

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.676244e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000655e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-13.0932821079632;6.36341374770037;0.668711861312787]
Robot Arm 1 end: [5.93809314817116;1.35896421348092;0.780173733631089]
Robot Arm 1 togoal: 9.57681629215991
Mobile Platform 2 Pose: [-10.0355620718106;-4.77334010022077;3.05098867314246]
Robot Arm 2 end: [-5.14418937358621;3.735283373785;0.459983821182502]
Robot Arm 1 togoal: 14.6069388144635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000649e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-13.1635356687206;6.39888784748297;0.675296681709551]
Robot Arm 1 end: [5.95044725765125;1.34979749950058;0.795651621910059]
Robot Arm 1 togoal: 9.57867480303557
Mobile Platform 2 Pose: [-10.1674651929108;-4.73737436390584;3.08341397024363]
Robot Arm 2 end: [-5.10313596453904;3.76132060446514;0.418912359206584]
Robot Arm 1 togoal: 14.6481336466001

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000612e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-13.2367178609603;6.43405927502342;0.684675655553164]
Robot Arm 1 end: [5.95708117870868;1.33603063642864;0.8211293079946]
Robot Arm 1 togoal: 9.58641626020813
Mobile Platform 2 Pose: [-10.3016099633409;-4.69995499333019;3.11581505630119]
Robot Arm 2 end: [-5.06568685700235;3.78444562467571;0.384228941452766]
Robot Arm 1 togoal: 14.6850480357523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000557e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-13.3119677955331;6.46646677290714;0.698914574001634]
Robot Arm 1 end: [5.96236556993883;1.32285335995005;0.855175027498921]
Robot Arm 1 togoal: 9.59367961200246
Mobile Platform 2 Pose: [-10.4369612064464;-4.66202125620685;3.14814064790843]
Robot Arm 2 end: [-5.03123178269787;3.80639336440161;0.356425082335613]
Robot Arm 1 togoal: 14.7194222056682

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000496e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-13.3788984815329;6.48726404614464;0.720243616371134]
Robot Arm 1 end: [5.98465644658304;1.33486072073676;0.894361491148926]
Robot Arm 1 togoal: 9.5708044803858
Mobile Platform 2 Pose: [-10.5725256466444;-4.62419650609007;3.18022031875747]
Robot Arm 2 end: [-4.99858498265419;3.82872466307657;0.335580907903003]
Robot Arm 1 togoal: 14.7530673558695

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000438e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-13.4210892741929;6.48663916001931;0.7485036136411]
Robot Arm 1 end: [6.05194723102584;1.4008349611005;0.93118275790807]
Robot Arm 1 togoal: 9.48054030168665
Mobile Platform 2 Pose: [-10.7074715795679;-4.58677863330099;3.2118191343792]
Robot Arm 2 end: [-4.96626677853237;3.85279919476813;0.321574310814569]
Robot Arm 1 togoal: 14.7877360559131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000373e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-13.4378426483138;6.46090400438296;0.793129622533954]
Robot Arm 1 end: [6.15482597760441;1.50492202128483;0.956833446923646]
Robot Arm 1 togoal: 9.34184120109989
Mobile Platform 2 Pose: [-10.8432840474911;-4.5492652832518;3.24311890844888]
Robot Arm 2 end: [-4.93692648186599;3.87714465003798;0.314268339031153]
Robot Arm 1 togoal: 14.8211355296985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000299e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-13.4228615947505;6.40605705947769;0.86839138816293]
Robot Arm 1 end: [6.23413332493614;1.62060793645038;0.959133910182843]
Robot Arm 1 togoal: 9.20389356044067
Mobile Platform 2 Pose: [-10.9804777000269;-4.51202598058854;3.27440854603686]
Robot Arm 2 end: [-4.91197772835478;3.90090019326281;0.312740541027312]
Robot Arm 1 togoal: 14.8520331180619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-13.3120714162437;6.30734411535346;0.941612313433371]
Robot Arm 1 end: [6.27286164405852;1.72632305389786;0.891315130186552]
Robot Arm 1 togoal: 9.09625162185305
Mobile Platform 2 Pose: [-11.1146226207314;-4.4777390585329;3.30576002817968]
Robot Arm 2 end: [-4.96108077485829;3.88078870416399;0.328143011521432]
Robot Arm 1 togoal: 14.8151918584307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985891e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-13.1531696369395;6.19765758173125;0.994292999869658]
Robot Arm 1 end: [6.30841591053323;1.78370532743038;0.773633241550947]
Robot Arm 1 togoal: 9.03846493093461
Mobile Platform 2 Pose: [-11.2370659279018;-4.46047561888247;3.33894226167812]
Robot Arm 2 end: [-5.04390456238508;3.82872864272624;0.353158387279225]
Robot Arm 1 togoal: 14.7363656270231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984465e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-12.984214962882;6.09360111413133;1.02890112364384]
Robot Arm 1 end: [6.34676311678208;1.77956504779299;0.65429483436091]
Robot Arm 1 togoal: 9.03730426468624
Mobile Platform 2 Pose: [-11.3253369983923;-4.48186763042695;3.37539112453448]
Robot Arm 2 end: [-5.11233023641588;3.80399217542383;0.346962741242762]
Robot Arm 1 togoal: 14.6907412740803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.010878e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.933546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-12.8142222022626;5.99479527329377;1.05417007951202]
Robot Arm 1 end: [6.3756994189583;1.73153563880011;0.561852774819437]
Robot Arm 1 togoal: 9.07871251850963
Mobile Platform 2 Pose: [-11.370852229677;-4.5378837963142;3.41164977380321]
Robot Arm 2 end: [-5.14426196663192;3.84719822661846;0.297791156796255]
Robot Arm 1 togoal: 14.7247862616085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.874492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000382e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-12.6414854937656;5.89775827905344;1.07486507592504]
Robot Arm 1 end: [6.3887676403691;1.65886023625603;0.503267951080134]
Robot Arm 1 togoal: 9.14611339515068
Mobile Platform 2 Pose: [-11.3250880233185;-4.63959270261414;3.44875864492487]
Robot Arm 2 end: [-5.14487936725364;3.97646646370195;0.258784308085875]
Robot Arm 1 togoal: 14.8494780925937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.971268e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820786e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-12.4648550551984;5.8007279175417;1.09262983667378]
Robot Arm 1 end: [6.38624307520356;1.57183472741683;0.477401397058351]
Robot Arm 1 togoal: 9.22942614779234
Mobile Platform 2 Pose: [-11.408134190752;-4.70324914663497;3.51242868669933]
Robot Arm 2 end: [-5.16855978172382;3.98521289712943;0.155291432322041]
Robot Arm 1 togoal: 14.859143792717

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.331482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-12.2840045281725;5.70275576457372;1.1078814743023]
Robot Arm 1 end: [6.36795812114252;1.47267310273442;0.477829331860214]
Robot Arm 1 togoal: 9.32712374022511
Mobile Platform 2 Pose: [-11.2696767367428;-4.80381490112914;3.52496427347519]
Robot Arm 2 end: [-5.20764726636133;4.135388844163;0.30799760915365]
Robot Arm 1 togoal: 14.9748868877031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.347589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-12.0714782568072;5.59390869710559;1.13204644397104]
Robot Arm 1 end: [6.34788450544237;1.27654045811801;0.502724308186924]
Robot Arm 1 togoal: 9.51176005526723
Mobile Platform 2 Pose: [-11.1964481277986;-4.98666685507502;3.62146595714005]
Robot Arm 2 end: [-5.26048709925845;4.30167623095623;0.311512510779103]
Robot Arm 1 togoal: 15.1150185424291

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-11.8045679420168;5.48355774896009;1.17996189929026]
Robot Arm 1 end: [6.25649049418629;1.08439822579282;0.543653539073552]
Robot Arm 1 togoal: 9.71890037612534
Mobile Platform 2 Pose: [-11.3318612141941;-5.03047111876713;3.73494998085737]
Robot Arm 2 end: [-5.366485723334;4.33590338008118;0.24005598122976]
Robot Arm 1 togoal: 15.1204628849223

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-11.5510647378516;5.39381266415758;1.22090427625112]
Robot Arm 1 end: [6.11079961688975;0.898747173528783;0.577641883156174]
Robot Arm 1 togoal: 9.94226368428993
Mobile Platform 2 Pose: [-11.095425218928;-5.26872093113094;3.81069314579667]
Robot Arm 2 end: [-5.47878846650812;4.49208379063564;0.322362977396527]
Robot Arm 1 togoal: 15.2282164759154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.912513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-11.3465442420082;5.35335893410331;1.24832494970271]
Robot Arm 1 end: [6.12179720967599;1.05380078837602;0.52770794128327]
Robot Arm 1 togoal: 9.80110026123844
Mobile Platform 2 Pose: [-10.9359811433543;-5.480136978609;3.94412198401685]
Robot Arm 2 end: [-5.56403866096573;4.60432871910973;0.261465049583787]
Robot Arm 1 togoal: 15.3150448457894

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960556e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-11.1319279106435;5.29853939282863;1.27517841306563]
Robot Arm 1 end: [6.0419330446201;0.942432496227817;0.64634875067615]
Robot Arm 1 togoal: 9.92324734734572
Mobile Platform 2 Pose: [-10.4683868072633;-5.70663652417431;3.8383015894702]
Robot Arm 2 end: [-5.65455705465;4.53133786736454;0.34178419352791]
Robot Arm 1 togoal: 15.2129288608416

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-11.0442904670875;5.32589810096064;1.27898811702055]
Robot Arm 1 end: [6.124327948406;1.09523861697547;0.659872146417968]
Robot Arm 1 togoal: 9.74972565359286
Mobile Platform 2 Pose: [-10.7628502417583;-5.52884215820161;3.69185204194365]
Robot Arm 2 end: [-5.62375334967747;4.34878102151797;0.262063056572061]
Robot Arm 1 togoal: 15.0540351326633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951294e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-10.9099294231685;5.35877208180527;1.29544880715876]
Robot Arm 1 end: [6.17585598076314;1.13325686024654;0.708593561431015]
Robot Arm 1 togoal: 9.69036715546209
Mobile Platform 2 Pose: [-10.3939658938259;-5.71946975208822;3.6219390308781]
Robot Arm 2 end: [-5.68604521459567;4.32909751659473;0.362567388220257]
Robot Arm 1 togoal: 15.0091707847779

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956613e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-10.7166802748931;5.29877692450652;1.30965934214766]
Robot Arm 1 end: [6.1023532146593;1.07642154720592;0.823298707284122]
Robot Arm 1 togoal: 9.76261696269524
Mobile Platform 2 Pose: [-10.6486727576832;-5.56795087377611;3.42308725312454]
Robot Arm 2 end: [-5.68316132089516;4.11945759057231;0.34377863066793]
Robot Arm 1 togoal: 14.8114830110221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956140e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-10.5705170622811;5.24028406917988;1.31743661482468]
Robot Arm 1 end: [6.04103202715405;1.23293041514718;0.961433068392237]
Robot Arm 1 togoal: 9.63576938633446
Mobile Platform 2 Pose: [-10.2016517876433;-5.81269550162013;3.37209639959532]
Robot Arm 2 end: [-5.79791086281203;4.10086555028806;0.423409416896789]
Robot Arm 1 togoal: 14.754547348946

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-10.4304207883938;5.18075621766869;1.3276546567364]
Robot Arm 1 end: [5.92221068910365;1.28925846472258;1.09536205389046]
Robot Arm 1 togoal: 9.62738549241881
Mobile Platform 2 Pose: [-9.88744627191115;-5.95819353798337;3.16879779777736]
Robot Arm 2 end: [-5.83789367269978;3.866575088156;0.349709592071843]
Robot Arm 1 togoal: 14.5250457821972

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.252330e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-10.2770346614198;5.12143786824005;1.34165986492835]
Robot Arm 1 end: [5.78211218370388;1.34255423712401;1.20975379850889]
Robot Arm 1 togoal: 9.63529028965611
Mobile Platform 2 Pose: [-9.87901072224067;-5.96719032018056;3.15383139309586]
Robot Arm 2 end: [-5.92674045765958;3.83111947307998;0.336099549536725]
Robot Arm 1 togoal: 14.4670417935177

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.058477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-10.1067741893127;5.06616530249971;1.36191111176542]
Robot Arm 1 end: [5.62668118722249;1.40049436283784;1.30670554085458]
Robot Arm 1 togoal: 9.65004335518747
Mobile Platform 2 Pose: [-9.84181898832416;-5.98898496036327;3.15912530634245]
Robot Arm 2 end: [-6.03087084378286;3.87437890225273;0.39222593942083]
Robot Arm 1 togoal: 14.4750304684254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-9.92049430129046;5.01865992503761;1.3909934286012]
Robot Arm 1 end: [5.44442679214092;1.47017911381265;1.39367396038496]
Robot Arm 1 togoal: 9.67095153141434
Mobile Platform 2 Pose: [-9.81539098899144;-6.00554797164343;3.16108722753948]
Robot Arm 2 end: [-6.08881525349285;3.90354781199901;0.492865193581148]
Robot Arm 1 togoal: 14.4797468576637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-9.75350577732441;5.01549676428793;1.43646052527678]
Robot Arm 1 end: [5.27540613555312;1.50310783273432;1.46293420405764]
Robot Arm 1 togoal: 9.72225978478092
Mobile Platform 2 Pose: [-9.76702555580456;-6.03753827506584;3.16676040858198]
Robot Arm 2 end: [-6.12740761932677;3.92383686218444;0.57825543948506]
Robot Arm 1 togoal: 14.4830581031064

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-9.7573605374719;5.0463990482091;1.44212390483505]
Robot Arm 1 end: [5.41805912949157;1.55767082388849;1.41210848968019]
Robot Arm 1 togoal: 9.60619390912337
Mobile Platform 2 Pose: [-9.68851567487809;-6.09158449240586;3.17779873687961]
Robot Arm 2 end: [-6.14532574464166;3.9389018880115;0.669730211261638]
Robot Arm 1 togoal: 14.4871032201663

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-9.83377400575788;5.13045853252782;1.44765972080553]
Robot Arm 1 end: [5.56918307757305;1.69700148304163;1.3234211743872]
Robot Arm 1 togoal: 9.41333949058507
Mobile Platform 2 Pose: [-9.58889800989422;-6.16715990228253;3.20279590906009]
Robot Arm 2 end: [-6.1457677816384;3.95033494314749;0.78794138155391]
Robot Arm 1 togoal: 14.4915260051223

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002881e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-10.0928163868443;5.31324289296345;1.4530429380163]
Robot Arm 1 end: [5.76614362040533;1.83531058533054;1.21953594458139]
Robot Arm 1 togoal: 9.20209615564625
Mobile Platform 2 Pose: [-9.59243571807344;-6.20374234627003;3.26899252889116]
Robot Arm 2 end: [-6.20726706594275;3.99679809888015;0.868428197445426]
Robot Arm 1 togoal: 14.5162333298755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.920611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-10.5810098686089;5.60206496248569;1.44718248979146]
Robot Arm 1 end: [6.08437275718302;2.03356804952783;0.838356488506022]
Robot Arm 1 togoal: 8.90293080339161
Mobile Platform 2 Pose: [-9.66154589387978;-6.20810947515506;3.33365142241152]
Robot Arm 2 end: [-6.33384345193965;4.03210426381096;0.927018031912929]
Robot Arm 1 togoal: 14.5152839615269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.749207e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-10.0054225655736;5.06826723791665;1.0635149739346]
Robot Arm 1 end: [5.12378340285049;1.1051981827637;0.720047717058456]
Robot Arm 1 togoal: 10.1752892946479
Mobile Platform 2 Pose: [-9.76098162644495;-6.20606873484495;3.38627568344251]
Robot Arm 2 end: [-6.45247400305403;4.04620728436553;0.953426873169446]
Robot Arm 1 togoal: 14.4983798759955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007622e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-10.0542189245216;5.08959297182474;1.03249471976131]
Robot Arm 1 end: [5.22878087343027;1.13002172123534;0.734334181771568]
Robot Arm 1 togoal: 10.1024694866686
Mobile Platform 2 Pose: [-9.86758817023426;-6.21151999481511;3.43242527272862]
Robot Arm 2 end: [-6.581267749673;4.08388504774686;0.837711238915616]
Robot Arm 1 togoal: 14.50897762658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-10.0600437864963;5.10552950831678;1.07900962889506]
Robot Arm 1 end: [5.33108734086182;1.18283875807065;0.853628933049102]
Robot Arm 1 togoal: 9.99932307338076
Mobile Platform 2 Pose: [-9.75359152799216;-6.31837659497604;3.45148309194403]
Robot Arm 2 end: [-6.51963904014407;4.07444521723925;1.02933241410986]
Robot Arm 1 togoal: 14.5067107780838

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.939217e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-10.0187744966452;5.086399312637;1.08673153724254]
Robot Arm 1 end: [5.39546852748305;1.19328907809584;0.849300870796806]
Robot Arm 1 togoal: 9.9604742388883
Mobile Platform 2 Pose: [-9.88828098288217;-6.31516560038237;3.50278741362182]
Robot Arm 2 end: [-6.65507739930301;4.16455610982391;0.836607301234431]
Robot Arm 1 togoal: 14.5702282191847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.556774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-9.98335228075608;5.07249142559782;1.09655816890552]
Robot Arm 1 end: [5.36222308218677;1.21663831188111;0.954519781413792]
Robot Arm 1 togoal: 9.94873444824845
Mobile Platform 2 Pose: [-9.76657430367916;-6.41946549962816;3.51908401033126]
Robot Arm 2 end: [-6.59828495042365;4.15001968217999;1.03164838917308]
Robot Arm 1 togoal: 14.5613937273743

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.733414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-9.952133808287;5.05870630113197;1.10196043266597]
Robot Arm 1 end: [5.41363885068068;1.26556338295679;0.945228676155346]
Robot Arm 1 togoal: 9.88213561061032
Mobile Platform 2 Pose: [-9.91289405520557;-6.42057575825639;3.57867213643581]
Robot Arm 2 end: [-6.75937332738332;4.2404393671577;0.836251394998191]
Robot Arm 1 togoal: 14.6205559146368

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.000001e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-9.96867868439179;5.06834816181437;1.10335551954482]
Robot Arm 1 end: [5.44138197719026;1.35604967420346;1.12155708830575]
Robot Arm 1 togoal: 9.78051277553619
Mobile Platform 2 Pose: [-9.80945796010516;-6.51092650592055;3.59314238813404]
Robot Arm 2 end: [-6.68938154723008;4.224265143614;1.03149601082261]
Robot Arm 1 togoal: 14.6126495739504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.272702e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-10.0317689461042;5.09871461592579;1.09873321779697]
Robot Arm 1 end: [5.43024415787342;1.48944039147195;1.10505086835321]
Robot Arm 1 togoal: 9.66878001544542
Mobile Platform 2 Pose: [-9.92577651881743;-6.53616763269024;3.65803622860074]
Robot Arm 2 end: [-6.77524722312576;4.30690926710641;0.968736642261522]
Robot Arm 1 togoal: 14.676228332425

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-10.0963545572537;5.1238901988811;1.06586125377195]
Robot Arm 1 end: [5.38290975612967;1.55731158364901;1.08665835912066]
Robot Arm 1 togoal: 9.63250552434991
Mobile Platform 2 Pose: [-9.93910314039079;-6.61693461575968;3.71543116912071]
Robot Arm 2 end: [-6.79192593263246;4.37854354621147;0.976289551260922]
Robot Arm 1 togoal: 14.7421492057631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-10.0781791479965;5.1224859871503;1.04714884281012]
Robot Arm 1 end: [5.28071918259656;1.46042994273809;1.21667238188052]
Robot Arm 1 togoal: 9.761582018072
Mobile Platform 2 Pose: [-9.88646970807083;-6.72671197420503;3.76937761299615]
Robot Arm 2 end: [-6.80628088038732;4.39463852716977;1.06416762427461]
Robot Arm 1 togoal: 14.7517509456461

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-10.0587779822976;5.11761249352789;1.03651172364331]
Robot Arm 1 end: [5.18550969682067;1.37731540466581;1.290084644204]
Robot Arm 1 togoal: 9.87842742604123
Mobile Platform 2 Pose: [-9.89036194958834;-6.80994422391606;3.83412431383434]
Robot Arm 2 end: [-6.92918294178113;4.47696615250519;1.02671862202508]
Robot Arm 1 togoal: 14.8073218532739

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.835600e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-10.0128286040535;5.09604744055985;1.02826140245915]
Robot Arm 1 end: [5.2468846961309;1.30018253341065;1.3151309103236]
Robot Arm 1 togoal: 9.91571031877857
Mobile Platform 2 Pose: [-9.77958947763278;-6.89910576357794;3.84689883719057]
Robot Arm 2 end: [-7.09963220304042;4.4223005634891;1.09114788744016]
Robot Arm 1 togoal: 14.7173251557452

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819575e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-9.92088015239258;5.04892404939848;1.00985410733805]
Robot Arm 1 end: [5.25733454728544;1.20988476267537;1.32742776753182]
Robot Arm 1 togoal: 9.98981840130248
Mobile Platform 2 Pose: [-9.68259351628188;-6.96658293817336;3.84558574802624]
Robot Arm 2 end: [-7.30811113771334;4.35785015555027;1.07746281485508]
Robot Arm 1 togoal: 14.6147477559739

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-9.8171165137023;4.99440680702983;0.984442450834302]
Robot Arm 1 end: [5.26299130527104;1.13018112243352;1.32036408712638]
Robot Arm 1 togoal: 10.0574943730686
Mobile Platform 2 Pose: [-9.58852306091284;-7.02256205316401;3.83768795283788]
Robot Arm 2 end: [-7.50810084152185;4.2766146674442;1.00583998957994]
Robot Arm 1 togoal: 14.5016094836071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-9.75499099853526;4.95979608373395;0.980917259600737]
Robot Arm 1 end: [5.24608204218961;1.08705424706558;1.33403346861496]
Robot Arm 1 togoal: 10.103231880354
Mobile Platform 2 Pose: [-9.48727178726591;-7.07972729735926;3.82805485460329]
Robot Arm 2 end: [-7.68122805795808;4.16471483034137;0.928106808100926]
Robot Arm 1 togoal: 14.3654923995349

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-9.72795305246104;4.93759113807373;0.987707255588477]
Robot Arm 1 end: [5.29091887900867;1.023506009145;1.31519987878183]
Robot Arm 1 togoal: 10.1387988964091
Mobile Platform 2 Pose: [-9.379013630282;-7.14266727045055;3.81817326007295]
Robot Arm 2 end: [-7.80706698297242;4.07410042133272;0.928066363866121]
Robot Arm 1 togoal: 14.2562557561159

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-9.81541997626863;4.96052627889847;1.01940745298915]
Robot Arm 1 end: [5.39454749328901;0.964621131983154;1.26055473588811]
Robot Arm 1 togoal: 10.144757060461
Mobile Platform 2 Pose: [-9.26409310061164;-7.21365398624671;3.80738705520754]
Robot Arm 2 end: [-7.9167436932903;3.96652814052487;0.958191317292837]
Robot Arm 1 togoal: 14.1322545530729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-9.97711500374146;4.9705696326441;1.11233661093484]
Robot Arm 1 end: [5.50690836802798;0.965627513310998;1.13918340978595]
Robot Arm 1 togoal: 10.097204689918
Mobile Platform 2 Pose: [-9.14076416122114;-7.29506427662416;3.79424381518948]
Robot Arm 2 end: [-8.03076037371343;3.8507354761447;0.964771766559159]
Robot Arm 1 togoal: 14.0010773793858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.203217e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-9.86326545296762;4.81484896772696;1.19474409452616]
Robot Arm 1 end: [5.45009066144435;1.06955494381292;0.971505477411736]
Robot Arm 1 togoal: 10.0377521448284
Mobile Platform 2 Pose: [-9.01490284061479;-7.39767759054239;3.77634191578724]
Robot Arm 2 end: [-8.15340189171829;3.72957234287059;0.965948577325871]
Robot Arm 1 togoal: 13.8643125820734

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.367784e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-9.57510009983347;4.58488798212907;1.21747843779482]
Robot Arm 1 end: [5.26989613395866;1.12778140020535;0.8187243207928]
Robot Arm 1 togoal: 10.0788559170021
Mobile Platform 2 Pose: [-8.97031761338797;-7.4933159158145;3.75731157391207]
Robot Arm 2 end: [-8.21445561665139;3.62678465947825;1.03060078416404]
Robot Arm 1 togoal: 13.7520151356662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-9.30951914395057;4.38476685267193;1.20958033907217]
Robot Arm 1 end: [5.0110368083339;1.13929575805473;0.637216084093233]
Robot Arm 1 togoal: 10.2070028608324
Mobile Platform 2 Pose: [-8.97549968511487;-7.5814776366313;3.72677135311472]
Robot Arm 2 end: [-8.26038642026398;3.63394273676662;1.1051292618607]
Robot Arm 1 togoal: 13.7507671941268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.948654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-9.18052887524508;4.29396473359532;1.20953738222505]
Robot Arm 1 end: [4.82599325464986;1.21274764801049;0.486056659731546]
Robot Arm 1 togoal: 10.2497442610067
Mobile Platform 2 Pose: [-8.8832260839528;-7.71125818652553;3.68881341502928]
Robot Arm 2 end: [-8.37328237763295;3.47007720026875;1.10737651411334]
Robot Arm 1 togoal: 13.5742504173231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975889e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-9.08259479450278;4.22966427041548;1.20881906342642]
Robot Arm 1 end: [4.64911077182528;1.2553856325082;0.330762347210672]
Robot Arm 1 togoal: 10.3207054816256
Mobile Platform 2 Pose: [-8.97569818654062;-7.73772242986149;3.66387534172783]
Robot Arm 2 end: [-8.34323682036167;3.60169021786843;1.19239585377278]
Robot Arm 1 togoal: 13.7061599910836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-9.00154309205122;4.18021042798349;1.20951656201259]
Robot Arm 1 end: [4.46437838956799;1.29227281124346;0.225586079619194]
Robot Arm 1 togoal: 10.3993132769817
Mobile Platform 2 Pose: [-8.83406749564909;-7.8589486393259;3.62716167854537]
Robot Arm 2 end: [-8.43354916845909;3.42020011506218;1.21723682284109]
Robot Arm 1 togoal: 13.5147257243256

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000377e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-8.8959748429086;4.11575572842577;1.2084501320293]
Robot Arm 1 end: [4.28049721422993;1.33886607652391;0.134392777886162]
Robot Arm 1 togoal: 10.4714197960562
Mobile Platform 2 Pose: [-8.83073134419464;-7.95809706919093;3.58016583700432]
Robot Arm 2 end: [-8.55139587961953;3.33984739072161;1.1862858881905]
Robot Arm 1 togoal: 13.4224444808675

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000252e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-8.77032969343688;4.04209341010764;1.21059328770964]
Robot Arm 1 end: [4.09326397484912;1.36930635735791;0.0560925785416095]
Robot Arm 1 togoal: 10.5605093142993
Mobile Platform 2 Pose: [-8.82754035911185;-8.05612454828397;3.53366830735248]
Robot Arm 2 end: [-8.65058771995476;3.30190626339147;1.19491477750894]
Robot Arm 1 togoal: 13.3741525082495

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958370e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-8.62637603669515;3.95904628647361;1.21672396357902]
Robot Arm 1 end: [4.02195465686974;1.29960123590152;0.0289750526894237]
Robot Arm 1 togoal: 10.66114924491
Mobile Platform 2 Pose: [-8.78063981243221;-8.17566124773197;3.48467685280053]
Robot Arm 2 end: [-8.7707555060101;3.1653068592974;1.16106799301417]
Robot Arm 1 togoal: 13.2274675495773

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998663e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-8.47767665338971;3.87226698333469;1.21490766398228]
Robot Arm 1 end: [3.93493717565747;1.2507034103587;0.0280767311172018]
Robot Arm 1 togoal: 10.750069663193
Mobile Platform 2 Pose: [-8.82554289341294;-8.25680561733899;3.44138181551343]
Robot Arm 2 end: [-8.9112973098979;3.19072118073882;1.12120493486024]
Robot Arm 1 togoal: 13.2416099817786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998922e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.632767e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-8.3252958349119;3.78271411075521;1.20777622500288]
Robot Arm 1 end: [3.8378996532089;1.23488347813823;0.0412163533549313]
Robot Arm 1 togoal: 10.8161225939158
Mobile Platform 2 Pose: [-8.72839831527036;-8.39276115439879;3.40333855647186]
Robot Arm 2 end: [-9.05888015409482;3.06297998161175;1.07064989774077]
Robot Arm 1 togoal: 13.1045781228909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.521022e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-8.17764269248017;3.69613367669561;1.19423362612076]
Robot Arm 1 end: [3.73940890128743;1.21166070758221;0.0723551346010873]
Robot Arm 1 togoal: 10.8870785966659
Mobile Platform 2 Pose: [-8.62393453586612;-8.5563894815892;3.37331832187451]
Robot Arm 2 end: [-9.11986421887322;2.91317774292526;1.12181683390251]
Robot Arm 1 togoal: 12.9492913170449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009265e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.318507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-8.041604524777;3.61819252751204;1.17316878984037]
Robot Arm 1 end: [3.66809870908899;1.23481583110897;0.12034320718136]
Robot Arm 1 togoal: 10.9033603500796
Mobile Platform 2 Pose: [-8.59641370956189;-8.78235713234826;3.31186562214435]
Robot Arm 2 end: [-9.31755488812445;2.74239325353848;1.03374427936207]
Robot Arm 1 togoal: 12.76995439678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-7.94839378534844;3.56879384645438;1.14633291321419]
Robot Arm 1 end: [3.63252825392308;1.19499665990247;0.164458916699542]
Robot Arm 1 togoal: 10.9504787094857
Mobile Platform 2 Pose: [-8.42994754485666;-9.04726023891293;3.2460408673912]
Robot Arm 2 end: [-9.34345047601812;2.49120349945655;0.918239937423757]
Robot Arm 1 togoal: 12.5229605858724

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.921032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-7.92295127763614;3.55909593927951;1.11496278925358]
Robot Arm 1 end: [3.60583382889793;1.17094671307457;0.171127090797524]
Robot Arm 1 togoal: 10.9845209198232
Mobile Platform 2 Pose: [-8.48214324702359;-9.03546651771746;3.24341370468841]
Robot Arm 2 end: [-9.29825197996284;2.44965311389246;0.903411971821908]
Robot Arm 1 togoal: 12.4846997525943

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.099526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-7.95625814312566;3.58556600575825;1.08839645145362]
Robot Arm 1 end: [3.59855084205857;1.14966654745018;0.164364224536693]
Robot Arm 1 togoal: 11.0066986044998
Mobile Platform 2 Pose: [-8.6120080960388;-9.04775475935061;3.20826726896134]
Robot Arm 2 end: [-9.4689219121814;2.45802244493417;0.814351691508139]
Robot Arm 1 togoal: 12.4893442184023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.527631e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-8.01973832998842;3.63051019386663;1.07251240477933]
Robot Arm 1 end: [3.62574021400308;1.13210586722215;0.164507552560088]
Robot Arm 1 togoal: 11.0050354806104
Mobile Platform 2 Pose: [-8.88159464183124;-8.97019812005399;3.20056815807368]
Robot Arm 2 end: [-9.56905285795766;2.57377628713216;0.685750593346666]
Robot Arm 1 togoal: 12.6088543147027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.039175e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-8.08647323546692;3.6756320330107;1.06456904078434]
Robot Arm 1 end: [3.61523272716093;1.14778704720296;0.124728457056221]
Robot Arm 1 togoal: 11.0034767919789
Mobile Platform 2 Pose: [-8.58224559898666;-9.1379892006959;3.17963084322121]
Robot Arm 2 end: [-9.56215800268644;2.43128769353504;0.820932552433961]
Robot Arm 1 togoal: 12.4586802577636

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985714e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-8.12098332232571;3.69917071731184;1.06272862899841]
Robot Arm 1 end: [3.5961823752468;1.18866574041148;0.101876333675651]
Robot Arm 1 togoal: 10.9846439899352
Mobile Platform 2 Pose: [-8.24206943713149;-9.33069086943826;3.1477955896905]
Robot Arm 2 end: [-9.55512216873383;2.3533128618748;0.941502239477982]
Robot Arm 1 togoal: 12.3748968724011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.496785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-8.12939069456162;3.70537228515323;1.06587422712204]
Robot Arm 1 end: [3.57765239823669;1.24823358764617;0.0976229503143618]
Robot Arm 1 togoal: 10.9483316665075
Mobile Platform 2 Pose: [-8.01538772967596;-9.52940096183135;3.08059332235582]
Robot Arm 2 end: [-9.64832317368561;2.22733061703901;0.818240733063816]
Robot Arm 1 togoal: 12.2525573328922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013611e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-8.12670787629813;3.70329804645655;1.06857930097665]
Robot Arm 1 end: [3.64777377078934;1.18778743586787;0.0857902560143312]
Robot Arm 1 togoal: 10.9574493086553
Mobile Platform 2 Pose: [-7.60947463886702;-9.7491410239116;3.01969673072248]
Robot Arm 2 end: [-9.67986440840849;2.19110851538382;0.924812452479205]
Robot Arm 1 togoal: 12.2098752337068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.269499e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-8.125348153296;3.70361563573299;1.07425628397865]
Robot Arm 1 end: [3.6688512275962;1.21572769780686;0.0582991014235186]
Robot Arm 1 togoal: 10.9264073956669
Mobile Platform 2 Pose: [-7.26554562035929;-9.80168766761938;3.13945374321793]
Robot Arm 2 end: [-9.65653192228502;2.35661977442545;0.977223462311206]
Robot Arm 1 togoal: 12.3733469802262

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.174482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-8.12863910890099;3.70767825811381;1.08042837747881]
Robot Arm 1 end: [3.73228355585285;1.2785708007807;0.0474624512317665]
Robot Arm 1 togoal: 10.8406138944645
Mobile Platform 2 Pose: [-7.80197929089814;-9.57887693002969;3.25283771646482]
Robot Arm 2 end: [-9.575405047302;2.50017503564358;1.06945255043521]
Robot Arm 1 togoal: 12.5155324255419

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.393783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-8.13916186896246;3.71873617630748;1.08907745589037]
Robot Arm 1 end: [3.8280650113864;1.35758941642121;0.0523980837121118]
Robot Arm 1 togoal: 10.7210463009611
Mobile Platform 2 Pose: [-8.30145878129102;-9.34362090086687;3.33742260399495]
Robot Arm 2 end: [-9.45318663214892;2.6658590442363;1.16369078685241]
Robot Arm 1 togoal: 12.6826913571814

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.901712e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-8.15573010650949;3.73771711204926;1.10406408935964]
Robot Arm 1 end: [3.91826935000057;1.31426860036887;0.0311068521348032]
Robot Arm 1 togoal: 10.70743476458
Mobile Platform 2 Pose: [-8.42588415684264;-9.29126180447881;3.34415317971251]
Robot Arm 2 end: [-9.29497240367026;2.74299580181285;1.25697394051927]
Robot Arm 1 togoal: 12.7652150658221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.703146e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.927929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-8.21892095005112;3.79061075019871;1.13632657544991]
Robot Arm 1 end: [4.05400385270315;1.44843407379786;0.0530908210963855]
Robot Arm 1 togoal: 10.5185030936612
Mobile Platform 2 Pose: [-8.56066652543502;-9.24288921828402;3.34420728537847]
Robot Arm 2 end: [-9.13879123805704;2.83097971766323;1.32713863128355]
Robot Arm 1 togoal: 12.8613103250413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.332604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999706e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-8.2066817690988;3.81333644681819;1.18479135470395]
Robot Arm 1 end: [4.17503404860324;1.58110050780052;0.121763616912364]
Robot Arm 1 togoal: 10.3327614629606
Mobile Platform 2 Pose: [-8.73584615767219;-9.18576191861471;3.32507023360921]
Robot Arm 2 end: [-9.00876031620368;2.90144998522548;1.37471938520851]
Robot Arm 1 togoal: 12.9403000680151

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.040552e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996922e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-8.06929560580972;3.7790053592438;1.25225329320149]
Robot Arm 1 end: [4.27928283583254;1.68620502884101;0.212100726340463]
Robot Arm 1 togoal: 10.1763946957385
Mobile Platform 2 Pose: [-8.96133131256474;-9.09592887149;3.28081635878244]
Robot Arm 2 end: [-8.87577043811525;2.93238660647606;1.40472997993094]
Robot Arm 1 togoal: 12.9816807141735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961919e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-7.78556359296423;3.71467141258417;1.37449901232852]
Robot Arm 1 end: [4.34454371033703;1.77763985480176;0.452642788295958]
Robot Arm 1 togoal: 10.0365720907804
Mobile Platform 2 Pose: [-9.23082503325862;-8.95232222504838;3.23274060871432]
Robot Arm 2 end: [-8.70377377407109;2.96322732163153;1.42854501197757]
Robot Arm 1 togoal: 13.0282006795217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.851924e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-8.05867680513498;3.92801122918804;1.4424882892001]
Robot Arm 1 end: [4.52947041452485;1.85590406073932;0.309763875678681]
Robot Arm 1 togoal: 9.88534701274073
Mobile Platform 2 Pose: [-9.40400460269575;-8.8408894471482;3.2186082889765]
Robot Arm 2 end: [-8.65460938335066;2.92342029771476;1.43598107088588]
Robot Arm 1 togoal: 12.9935405614121

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-8.22522269989788;4.12859807720054;1.59170261962993]
Robot Arm 1 end: [4.54360862189065;2.1038555634637;0.154989022987165]
Robot Arm 1 togoal: 9.6947042153471
Mobile Platform 2 Pose: [-9.61873450747994;-8.68657128621156;3.20323575296954]
Robot Arm 2 end: [-8.58768499323715;2.89893168794733;1.4466335191918]
Robot Arm 1 togoal: 12.976232224422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.358246e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-7.91089801600188;4.02398371662906;1.68918609903204]
Robot Arm 1 end: [4.62250190901567;2.10747317168306;0.310496429014559]
Robot Arm 1 togoal: 9.62677434811948
Mobile Platform 2 Pose: [-9.85728710283299;-8.47954823385596;3.20567183513793]
Robot Arm 2 end: [-8.48064305421148;2.90327243914909;1.45143279963307]
Robot Arm 1 togoal: 12.9926026939558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.392927e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-8.01219327885785;4.20808497817457;1.85832132748157]
Robot Arm 1 end: [4.62158449921921;2.37132429440181;0.154569331522761]
Robot Arm 1 togoal: 9.43351361328208
Mobile Platform 2 Pose: [-9.81696981857609;-8.47263037091031;3.16230254967907]
Robot Arm 2 end: [-8.523603234984;2.78443452577607;1.41182615613373]
Robot Arm 1 togoal: 12.8698652860001

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.856970e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [-7.68438416324505;4.0972262897368;1.96211477734285]
Robot Arm 1 end: [4.69369439273268;2.38346576664275;0.310671372839026]
Robot Arm 1 togoal: 9.36126958864794
Mobile Platform 2 Pose: [-9.73381203721767;-8.49695708182885;3.13595693600418]
Robot Arm 2 end: [-8.59146504484238;2.73328612534045;1.39433683121528]
Robot Arm 1 togoal: 12.8115803017432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.239091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [-7.77718061033601;4.27526174902773;2.13762368095029]
Robot Arm 1 end: [4.68652739304068;2.6525569924154;0.154721579925524]
Robot Arm 1 togoal: 9.16976698808493
Mobile Platform 2 Pose: [-9.63730883123086;-8.52568902522135;3.10766266716357]
Robot Arm 2 end: [-8.56450786117883;2.78118235371945;1.43231068398872]
Robot Arm 1 togoal: 12.8618476835337

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.136431e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [-7.43690802768843;4.15553153525456;2.243937066296]
Robot Arm 1 end: [4.74917747398107;2.66964955931215;0.311153535437346]
Robot Arm 1 togoal: 9.09774165663408
Mobile Platform 2 Pose: [-9.63510190913399;-8.42190774491498;3.06495012108694]
Robot Arm 2 end: [-8.37142087338959;2.8255165633274;1.43680873903387]
Robot Arm 1 togoal: 12.9287753966197

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.443531e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [-7.4877473579908;4.31587842227783;2.43574133064366]
Robot Arm 1 end: [4.76269433962339;2.92008182525762;0.154634839287385]
Robot Arm 1 togoal: 8.91187778718209
Mobile Platform 2 Pose: [-9.86295867470624;-8.17315765787001;3.038047819811]
Robot Arm 2 end: [-7.89088179614737;2.86023000200745;1.39480336970717]
Robot Arm 1 togoal: 13.032644432035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.725192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [-7.12811014396544;4.18834443313314;2.55052245465275]
Robot Arm 1 end: [4.80824021030521;2.93844101439652;0.31276145554689]
Robot Arm 1 togoal: 8.8475886772245
Mobile Platform 2 Pose: [-10.2467634765714;-7.94774883171876;3.03116345437855]
Robot Arm 2 end: [-7.67731797539129;2.88584601617028;1.37497213140722]
Robot Arm 1 togoal: 13.0943195118455

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.782492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [-7.07481668107568;4.2954066227913;2.76860552289495]
Robot Arm 1 end: [4.97561163528399;3.09570979889105;0.384542062570771]
Robot Arm 1 togoal: 8.61424623888641
Mobile Platform 2 Pose: [-10.6247156127501;-7.72065003514611;3.04390304807794]
Robot Arm 2 end: [-7.46731842455979;2.95198713673784;1.35356595103241]
Robot Arm 1 togoal: 13.1983514468115

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949536e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [-7.28121716782142;4.50214562174026;2.90917218435351]
Robot Arm 1 end: [5.1293206550177;3.29246051835725;0.262800891481257]
Robot Arm 1 togoal: 8.38437046987493
Mobile Platform 2 Pose: [-10.9092726845985;-7.55835881253022;3.04373170039328]
Robot Arm 2 end: [-7.33837068816671;2.99485080697625;1.33347545214422]
Robot Arm 1 togoal: 13.2659558097119

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [-7.38618651118847;4.66748838900228;3.09668690194695]
Robot Arm 1 end: [5.13060402535499;3.59776385905852;0.147575611112779]
Robot Arm 1 togoal: 8.16002079128825
Mobile Platform 2 Pose: [-11.1610411420686;-7.41197745820716;3.0469286028443]
Robot Arm 2 end: [-7.18012582254613;3.08717427013771;1.2903501499403]
Robot Arm 1 togoal: 13.3895115708885

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958092e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [-7.08323039473457;4.54346160793334;3.17345356173268]
Robot Arm 1 end: [5.1809128362798;3.58056920840036;0.307479593075912]
Robot Arm 1 togoal: 8.1182094428323
Mobile Platform 2 Pose: [-11.3626814280238;-7.29374499071074;3.05430286844545]
Robot Arm 2 end: [-7.01404965493324;3.2308560882417;1.19689593649997]
Robot Arm 1 togoal: 13.5674793434501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.262820e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008632e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [-6.80812690389597;4.46718228747348;3.29529883463266]
Robot Arm 1 end: [5.1811633872976;3.60571467278224;0.448032955637977]
Robot Arm 1 togoal: 8.07832466691457
Mobile Platform 2 Pose: [-11.5101840938182;-7.20762066549399;3.06047423219753]
Robot Arm 2 end: [-6.95988890530346;3.38578979612951;1.12249560997541]
Robot Arm 1 togoal: 13.7324597098674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.390197e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.696397e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [-6.87431434402095;4.63093040895536;3.4787509043314]
Robot Arm 1 end: [5.27292680186379;3.8785227407879;0.311587956595213]
Robot Arm 1 togoal: 7.82817873750473
Mobile Platform 2 Pose: [-11.6445142713299;-7.1278408521589;3.06404499029915]
Robot Arm 2 end: [-6.90153703620197;3.46461856761227;1.11584731123262]
Robot Arm 1 togoal: 13.8224662998516

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.686440e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.617892e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-6.5822966244292;4.54839370667884;3.59644072098425]
Robot Arm 1 end: [5.2559059815441;3.93573899827561;0.504933399887731]
Robot Arm 1 togoal: 7.76621406406287
Mobile Platform 2 Pose: [-11.8020198792498;-7.02957946996843;3.06390231760958]
Robot Arm 2 end: [-6.8240290546149;3.55134980100881;1.10568974354321]
Robot Arm 1 togoal: 13.9247389736523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.026745e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.099013e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-6.81278335514995;4.75443696150578;3.71012749350861]
Robot Arm 1 end: [5.42424383407697;4.11239829463051;0.376605663407841]
Robot Arm 1 togoal: 7.54394039800663
Mobile Platform 2 Pose: [-11.9680925410817;-6.92099377843968;3.05538339767358]
Robot Arm 2 end: [-6.68023984316496;3.61325532148651;1.11600756708016]
Robot Arm 1 togoal: 14.018043615912

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.125070e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-6.99768069599714;4.94946196078115;3.8539864720737]
Robot Arm 1 end: [5.45229459546877;4.37654555623681;0.213665987225951]
Robot Arm 1 togoal: 7.34942083081643
Mobile Platform 2 Pose: [-12.1269686914068;-6.81229605289661;3.03337685223115]
Robot Arm 2 end: [-6.51438925691947;3.62612686808998;1.14029054295628]
Robot Arm 1 togoal: 14.0700303968486

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.237113e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [-6.69199119288588;4.83820072459872;3.94329731942109]
Robot Arm 1 end: [5.5043049223908;4.35304583967425;0.378476205852511]
Robot Arm 1 togoal: 7.30785373005324
Mobile Platform 2 Pose: [-12.2594473122261;-6.70467942257804;2.98038791034152]
Robot Arm 2 end: [-6.39120718756405;3.57256274019945;1.18727780560372]
Robot Arm 1 togoal: 14.0481036230081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [-6.59662219016354;4.90351164990288;4.1173756947549]
Robot Arm 1 end: [5.63368769463679;4.46307234353852;0.502737669572732]
Robot Arm 1 togoal: 7.12454273581269
Mobile Platform 2 Pose: [-12.464089160104;-6.50969225988417;2.82392703221274]
Robot Arm 2 end: [-6.05575260227618;3.40906809098969;1.10414884852518]
Robot Arm 1 togoal: 13.9833457901081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [-6.83662724687582;5.11354598655764;4.21548479104852]
Robot Arm 1 end: [5.80566974484002;4.62468169883935;0.379660384448708]
Robot Arm 1 togoal: 6.91296674707537
Mobile Platform 2 Pose: [-12.5922613655634;-6.37506038128493;2.6132750508195]
Robot Arm 2 end: [-6.05489570793949;3.2037919235471;1.18731759831186]
Robot Arm 1 togoal: 13.7846042010918

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [-6.78250736531612;5.20688500249076;4.35167171146076]
Robot Arm 1 end: [5.94931744478616;4.68702658166791;0.497404669912167]
Robot Arm 1 togoal: 6.75895429092361
Mobile Platform 2 Pose: [-12.9212852741121;-6.14699813761572;2.51086600330627]
Robot Arm 2 end: [-5.6970973017127;3.07964939856083;1.12939357813739]
Robot Arm 1 togoal: 13.7748159918688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [-7.06070538887107;5.44315767625963;4.43742595926699]
Robot Arm 1 end: [6.04587501539259;4.95497500557944;0.335757426043144]
Robot Arm 1 togoal: 6.51860272967639
Mobile Platform 2 Pose: [-13.0857354909305;-5.9985430281955;2.30971660485023]
Robot Arm 2 end: [-5.68317016335649;2.88018867790718;1.21085371657089]
Robot Arm 1 togoal: 13.5878795598927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002415e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [-6.92465769418651;5.43457790767322;4.51047780075195]
Robot Arm 1 end: [6.09284755437849;4.97449791901272;0.449718232003736]
Robot Arm 1 togoal: 6.45516506572643
Mobile Platform 2 Pose: [-13.3868574766739;-5.7773287060496;2.19325105768053]
Robot Arm 2 end: [-5.32220865273163;2.75743405659728;1.12988409779172]
Robot Arm 1 togoal: 13.5936318637062

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989702e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [-7.05198006369391;5.62942987220836;4.66710715655172]
Robot Arm 1 end: [6.21787662681087;5.18232916505685;0.332473085229528]
Robot Arm 1 togoal: 6.23915104088843
Mobile Platform 2 Pose: [-13.6654149403282;-5.58085316610236;2.03866334082543]
Robot Arm 2 end: [-5.15329470646165;2.53457506075363;1.20735196152716]
Robot Arm 1 togoal: 13.4426373620699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002759e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [-6.86636912718765;5.6184296376759;4.77703545727095]
Robot Arm 1 end: [6.26577513189286;5.23370531399075;0.448075631591041]
Robot Arm 1 togoal: 6.14924118081212
Mobile Platform 2 Pose: [-14.0365555088298;-5.35680701459902;2.00677125190409]
Robot Arm 2 end: [-4.91714448685559;2.4335553477412;1.25202452378349]
Robot Arm 1 togoal: 13.4350685357259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969592e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [-7.12727656524732;5.83970871878273;4.89337957585199]
Robot Arm 1 end: [6.4308267542387;5.40542953922449;0.332468608326881]
Robot Arm 1 togoal: 5.93815478463483
Mobile Platform 2 Pose: [-14.3853609128037;-5.1495192130715;1.98903318040267]
Robot Arm 2 end: [-4.74226515723918;2.39137819937243;1.28778746936879]
Robot Arm 1 togoal: 13.4627046852075

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001997e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [-6.98091565913102;5.86669359233361;5.0264312372409]
Robot Arm 1 end: [6.52195908554712;5.48043589860571;0.446454239876937]
Robot Arm 1 togoal: 5.80326432792947
Mobile Platform 2 Pose: [-14.7005032176025;-4.96626855667818;2.0002488039177]
Robot Arm 2 end: [-4.65120521698108;2.47061361284015;1.29413395035626]
Robot Arm 1 togoal: 13.5711929363666

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969361e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.228802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [-7.26862784804499;6.08918562204694;5.1224702028217]
Robot Arm 1 end: [6.68098129683854;5.63812496071224;0.328376267621333]
Robot Arm 1 togoal: 5.60929499824443
Mobile Platform 2 Pose: [-15.0569478902921;-4.76526603535406;2.0541180061964]
Robot Arm 2 end: [-4.52225657203735;2.58699359079098;1.27896196214374]
Robot Arm 1 togoal: 13.7294086955328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000064e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966377e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [-7.24808971993111;6.19637942275424;5.27905808221924]
Robot Arm 1 end: [6.86700128112658;5.74037042689605;0.441905286768005]
Robot Arm 1 togoal: 5.39671849114344
Mobile Platform 2 Pose: [-15.3676559924244;-4.60512601611112;2.1668553356599]
Robot Arm 2 end: [-4.43820697602092;2.82636618874545;1.18005094143819]
Robot Arm 1 togoal: 13.9844720139843

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002468e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [-7.50264203678558;6.41198414687256;5.37712580013713]
Robot Arm 1 end: [7.01471932665165;5.88758649429579;0.361304955620337]
Robot Arm 1 togoal: 5.21236396815536
Mobile Platform 2 Pose: [-15.3255408617155;-4.63633616288401;2.21292967499928]
Robot Arm 2 end: [-4.32442633551557;2.87003713856487;1.0927865173118]
Robot Arm 1 togoal: 14.0724325955317

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001990e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [-7.58794566496055;6.57647128413186;5.51685495032052]
Robot Arm 1 end: [7.24578448082976;5.95849694512303;0.39540963948779]
Robot Arm 1 togoal: 5.01860575541615
Mobile Platform 2 Pose: [-15.2350484186683;-4.69860543201933;2.28719108658466]
Robot Arm 2 end: [-4.20815971355557;2.86472838318798;1.01019217800411]
Robot Arm 1 togoal: 14.1176334740598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999555e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [-7.82823268737598;6.79413622935472;5.60860591521647]
Robot Arm 1 end: [7.38431452359107;6.12429874910131;0.367289125128869]
Robot Arm 1 togoal: 4.81600659058002
Mobile Platform 2 Pose: [-15.1569327535047;-4.7553632628802;2.43910444885452]
Robot Arm 2 end: [-4.13697047409347;2.91394176443098;0.911611633445073]
Robot Arm 1 togoal: 14.1956458570246

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000522e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999990e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [-7.8448018927183;6.91066845103765;5.72766206487295]
Robot Arm 1 end: [7.53501426975274;6.15987071391632;0.292154202368645]
Robot Arm 1 togoal: 4.72576021170313
Mobile Platform 2 Pose: [-14.9613341968079;-4.86493517761189;2.55603791972193]
Robot Arm 2 end: [-4.17211443251176;2.93324887784648;0.827926957071795]
Robot Arm 1 togoal: 14.2025887417361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994719e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [-8.12978108437936;7.14592001564368;5.83135197135426]
Robot Arm 1 end: [7.58963830932452;6.3971783674859;0.200509830305675]
Robot Arm 1 togoal: 4.531430401523
Mobile Platform 2 Pose: [-14.7697532143104;-4.97250198027326;2.5942167538292]
Robot Arm 2 end: [-4.23093038364188;2.81929210084896;0.800310820505176]
Robot Arm 1 togoal: 14.0760721473967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000490e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [-8.04597836943737;7.159693696922;5.91352350311732]
Robot Arm 1 end: [7.7440732859193;6.3013226957625;0.162749113793579]
Robot Arm 1 togoal: 4.54029344977448
Mobile Platform 2 Pose: [-14.5266069853034;-5.11254484331599;2.64258405472118]
Robot Arm 2 end: [-4.35209662478944;2.71980848172718;0.767301967818033]
Robot Arm 1 togoal: 13.9377329961242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000032e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.889775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [-8.30134759483684;7.3883596633989;6.02790532621824]
Robot Arm 1 end: [7.8384614824886;6.55451619962015;0.142517340305171]
Robot Arm 1 togoal: 4.29462708793728
Mobile Platform 2 Pose: [-14.1849783987312;-5.3147307876332;2.72543367606413]
Robot Arm 2 end: [-4.55211395631634;2.65859009125412;0.637638133981792]
Robot Arm 1 togoal: 13.8094059839551

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000180e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002928e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [-8.20893206318647;7.37927868528813;6.08199928906657]
Robot Arm 1 end: [7.96737216006379;6.45079370318896;0.17042031256097]
Robot Arm 1 togoal: 4.30726209617314
Mobile Platform 2 Pose: [-13.9784184698692;-5.36957359752156;2.95377814987749]
Robot Arm 2 end: [-4.61812531306576;2.76015630858152;0.594278914386881]
Robot Arm 1 togoal: 13.879660519473

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000753e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.834478e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [-8.15722314094742;7.51144301209565;6.27222581561984]
Robot Arm 1 end: [8.16835217904135;6.41942291388233;0.167064262735372]
Robot Arm 1 togoal: 4.24365502724901
Mobile Platform 2 Pose: [-13.9747635122264;-5.24856496249009;3.17715776813974]
Robot Arm 2 end: [-4.5484942851978;2.96445550386731;0.436105426770404]
Robot Arm 1 togoal: 14.1057795709879

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000466e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.356784e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [-7.91824067109699;7.51561939541114;6.40263703640816]
Robot Arm 1 end: [8.23047673030385;6.46788270862664;0.287247385850085]
Robot Arm 1 togoal: 4.1387450603941
Mobile Platform 2 Pose: [-14.2044034245439;-5.01336315232858;3.29182550870775]
Robot Arm 2 end: [-4.30336598876891;3.15436141460975;0.371386703889195]
Robot Arm 1 togoal: 14.380906578331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993486e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.863523e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [-8.30622928985969;7.76664868944338;6.43180198509297]
Robot Arm 1 end: [8.25022769487602;6.69075950148161;0.0155583749560502]
Robot Arm 1 togoal: 4.03474042327732
Mobile Platform 2 Pose: [-14.5217344943334;-4.75398392965268;3.36171557307263]
Robot Arm 2 end: [-4.05620365523842;3.41920340247684;0.334781320961176]
Robot Arm 1 togoal: 14.7244982834577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.197639e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.475571e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [-8.65934901033124;8.05735123024772;6.57586574712773]
Robot Arm 1 end: [8.28096542690649;7.04076157260351;-0.0493116319863623]
Robot Arm 1 togoal: 3.76537522104006
Mobile Platform 2 Pose: [-14.6398687842932;-4.58030112871458;3.54750251190521]
Robot Arm 2 end: [-3.88857083789939;3.53875447990727;0.309429569410636]
Robot Arm 1 togoal: 14.9035345532146

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.922137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [-8.59039674667337;8.04360056979841;6.61780801389426]
Robot Arm 1 end: [8.38516368357353;7.0984216545625;0.0144392647207462]
Robot Arm 1 togoal: 3.64644737692
Mobile Platform 2 Pose: [-14.8756426639339;-4.36507217355269;3.65158414153241]
Robot Arm 2 end: [-3.66713655354125;3.64925520851783;0.339619278110407]
Robot Arm 1 togoal: 15.0931437278255

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010150e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [-8.48419619109321;8.01170843960336;6.65947722412291]
Robot Arm 1 end: [8.4821122026291;7.14283384022307;0.128530246624036]
Robot Arm 1 togoal: 3.52226544202007
Mobile Platform 2 Pose: [-15.2062167455431;-4.11991328968431;3.7005445717836]
Robot Arm 2 end: [-3.44137473205506;3.84541562895064;0.279539224767537]
Robot Arm 1 togoal: 15.3705017549902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.014695e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [-8.19999267437236;7.92185166930391;6.75889149537149]
Robot Arm 1 end: [8.61543108910471;7.03308127699037;0.292583581644565]
Robot Arm 1 togoal: 3.49694790851223
Mobile Platform 2 Pose: [-15.3473528915563;-3.92867040096378;3.89781957202347]
Robot Arm 2 end: [-3.26782849599319;4.00025499187796;0.21603934359883]
Robot Arm 1 togoal: 15.5894898990224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013495e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [-8.49906382198457;8.14284254126978;6.83444717085292]
Robot Arm 1 end: [8.602379600619;7.31060595648196;0.11401120060671]
Robot Arm 1 togoal: 3.34152668457015
Mobile Platform 2 Pose: [-15.6344451431034;-3.6606563201876;4.0645444593689]
Robot Arm 2 end: [-3.01581106550802;4.26356740565652;0.256318179908123]
Robot Arm 1 togoal: 15.9319700700967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.021000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [-8.14260795543206;8.01574653756171;6.94835535479756]
Robot Arm 1 end: [8.64696531418522;7.33573379999715;0.268138374646402]
Robot Arm 1 togoal: 3.24016658488097
Mobile Platform 2 Pose: [-15.9255017016721;-3.42311131876494;4.19703360191248]
Robot Arm 2 end: [-2.87568468853578;4.54739938427606;0.182577681094513]
Robot Arm 1 togoal: 16.2534326161135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.014434e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [-7.8581958649418;7.9501555738878;7.07460984246944]
Robot Arm 1 end: [8.63708715059467;7.40452980731407;0.392718644192016]
Robot Arm 1 togoal: 3.14118063378393
Mobile Platform 2 Pose: [-15.6617837628179;-3.56838497637886;4.24843045010751]
Robot Arm 2 end: [-2.9387076436266;4.46648742011192;0.242934482638644]
Robot Arm 1 togoal: 16.1485157053242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.025881e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [-8.01551805625121;8.15680452088062;7.24814817199355]
Robot Arm 1 end: [8.74598073326016;7.6005553290459;0.175674274090321]
Robot Arm 1 togoal: 3.02321027371884
Mobile Platform 2 Pose: [-15.3223463688241;-3.75412611154131;4.31320693301053]
Robot Arm 2 end: [-3.09514622953682;4.44877269455181;0.284836892996083]
Robot Arm 1 togoal: 16.0615110496311

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009517e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [-7.64191565465199;8.02934647937318;7.38256023482568]
Robot Arm 1 end: [8.68997299651734;7.69828177561023;0.294044130925763]
Robot Arm 1 togoal: 2.91881795235631
Mobile Platform 2 Pose: [-14.982791467942;-3.95071310220203;4.35929548389993]
Robot Arm 2 end: [-3.29847265032251;4.33901142052895;0.24163404221148]
Robot Arm 1 togoal: 15.8793732429993

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.377020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [-7.44597602713905;8.02969564532278;7.56280642538924]
Robot Arm 1 end: [8.62997135571393;7.93244104590864;0.232427972841648]
Robot Arm 1 togoal: 2.79503065886274
Mobile Platform 2 Pose: [-14.5378733355491;-4.11529849536916;4.56977644782451]
Robot Arm 2 end: [-3.50014699655705;4.28380343579969;0.355194202269677]
Robot Arm 1 togoal: 15.7363983432344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.378313e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [-7.1764339803769;7.89529857152152;7.59047596760219]
Robot Arm 1 end: [8.43709710379418;8.15244828086954;0.348747572331319]
Robot Arm 1 togoal: 2.68891959195615
Mobile Platform 2 Pose: [-14.8243475653083;-3.86964946170515;4.58707474119267]
Robot Arm 2 end: [-3.1896189322858;4.38158618330868;0.407451791446121]
Robot Arm 1 togoal: 15.9515297400653

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.442429e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [-7.00299402957699;7.80972101045931;7.6071160892894]
Robot Arm 1 end: [8.27101644717059;8.31186369544905;0.510622035251394]
Robot Arm 1 togoal: 2.61917008618133
Mobile Platform 2 Pose: [-15.0705468808247;-3.62211893504238;4.62212680149001]
Robot Arm 2 end: [-2.96003141099134;4.34335915802571;0.389394021383887]
Robot Arm 1 togoal: 16.0179162740098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003882e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [-6.74842116980463;7.70091872910493;7.65368149070886]
Robot Arm 1 end: [8.00309212588379;8.46724547705482;0.667797914053997]
Robot Arm 1 togoal: 2.65799384588807
Mobile Platform 2 Pose: [-15.2989595087363;-3.37265510059333;4.60576234631532]
Robot Arm 2 end: [-2.70059162904847;4.4148599904103;0.311373048678766]
Robot Arm 1 togoal: 16.2028623542389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.196480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [-6.30230759293481;7.49898687623399;7.71664995162699]
Robot Arm 1 end: [7.56538761614392;8.36241510931157;1.15572215592852]
Robot Arm 1 togoal: 2.95676337186558
Mobile Platform 2 Pose: [-15.4980343844252;-3.12266712930116;4.60330995714135]
Robot Arm 2 end: [-2.44155986913411;4.59313637691238;0.234107699293172]
Robot Arm 1 togoal: 16.4847122607787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.831825e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [-6.94900174818195;7.8319536299393;7.72977865335067]
Robot Arm 1 end: [7.88273686559446;8.59050429508157;0.567278782709143]
Robot Arm 1 togoal: 2.71644353572958
Mobile Platform 2 Pose: [-15.747642965296;-2.87964500683433;4.66196366162269]
Robot Arm 2 end: [-2.21843987056385;4.71993275840641;0.185249851924019]
Robot Arm 1 togoal: 16.7036920145853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [-7.24903341195707;8.00245283141806;7.7273442688973]
Robot Arm 1 end: [7.89152993036563;8.57395501862701;0.685548957888525]
Robot Arm 1 togoal: 2.67903504404258
Mobile Platform 2 Pose: [-15.9983355162385;-2.63595087900128;4.71775948737142]
Robot Arm 2 end: [-1.97527173965833;4.94227666036823;0.198584222465477]
Robot Arm 1 togoal: 17.0122508486086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.888110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [-7.31613289001482;8.04206428097114;7.72929552289739]
Robot Arm 1 end: [7.94434771784605;8.52738989538618;0.829738481846772]
Robot Arm 1 togoal: 2.62147464451117
Mobile Platform 2 Pose: [-16.3619327769875;-2.39081531323841;4.76683651252863]
Robot Arm 2 end: [-1.76741543197679;5.06888614914581;0.175316151150441]
Robot Arm 1 togoal: 17.2237523050765

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.875822e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [-7.2745643940976;8.02129359377922;7.73470824894612]
Robot Arm 1 end: [7.97957261989103;8.47741187180382;0.919676025448535]
Robot Arm 1 togoal: 2.60038303501394
Mobile Platform 2 Pose: [-16.7000396351356;-2.16965990647588;4.81360467443107]
Robot Arm 2 end: [-1.62330657136796;5.27245482174693;0.199879244032037]
Robot Arm 1 togoal: 17.468893185644

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.914793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [-7.14748186614604;7.9527147186304;7.74072890723429]
Robot Arm 1 end: [8.01208359109521;8.48641419427174;0.838716936398214]
Robot Arm 1 togoal: 2.59003162509125
Mobile Platform 2 Pose: [-16.9546038441096;-2.01557686128681;4.85248811677459]
Robot Arm 2 end: [-1.54274324267515;5.38265898417138;0.1866995208222]
Robot Arm 1 togoal: 17.6048785014654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.929162e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [-7.02007662506963;7.88410680830187;7.7476154863493]
Robot Arm 1 end: [8.05096751640527;8.45706153808161;0.760001083016661]
Robot Arm 1 togoal: 2.5997126900918
Mobile Platform 2 Pose: [-17.1870312487379;-1.88083647436379;4.89226067242093]
Robot Arm 2 end: [-1.51104834512945;5.50470034785626;0.1886655210523]
Robot Arm 1 togoal: 17.726622584106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006105e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [-6.8865677510184;7.8116599351448;7.75860877267583]
Robot Arm 1 end: [8.11755095346897;8.41139193001715;0.62625262471431]
Robot Arm 1 togoal: 2.62066077132864
Mobile Platform 2 Pose: [-17.3855639251417;-1.77292301839987;4.92996005512276]
Robot Arm 2 end: [-1.51545879029161;5.60039339520172;0.187212050676275]
Robot Arm 1 togoal: 17.8083885828576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.251463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.926700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [-6.7271167196732;7.72535645093559;7.80782048655453]
Robot Arm 1 end: [8.06148231508253;8.4162583099439;0.424895455010864]
Robot Arm 1 togoal: 2.73267885583092
Mobile Platform 2 Pose: [-17.5595396310042;-1.68544555475794;4.96781705664764]
Robot Arm 2 end: [-1.54761054475498;5.6747502184;0.184689841031381]
Robot Arm 1 togoal: 17.8585107652719

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.705167e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [-6.2792605574195;7.49171736739284;7.86878523536035]
Robot Arm 1 end: [7.73995451410735;8.44367795469446;0.225191864633633]
Robot Arm 1 togoal: 3.03464374032429
Mobile Platform 2 Pose: [-17.7126669397151;-1.61701146198814;5.00799823500202]
Robot Arm 2 end: [-1.60206104406746;5.72948398160209;0.181315325764897]
Robot Arm 1 togoal: 17.8811856447336

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.005436e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [-5.80938788587467;7.24590715266476;7.9143630625943]
Robot Arm 1 end: [7.36004579881185;8.53912909133273;0.229272173882781]
Robot Arm 1 togoal: 3.28208207894422
Mobile Platform 2 Pose: [-17.8567977957147;-1.56447786381156;5.05339167454583]
Robot Arm 2 end: [-1.70970122159242;5.76758787900625;0.194626829257581]
Robot Arm 1 togoal: 17.863514411938

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.917162e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.449363e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [-5.40184756942567;7.02068715600019;7.92112301783176]
Robot Arm 1 end: [6.97339645625828;8.53608997783916;0.183230053847029]
Robot Arm 1 togoal: 3.6184236889567
Mobile Platform 2 Pose: [-17.8704444666902;-1.5980435183209;5.10199217817689]
Robot Arm 2 end: [-1.86020265721422;5.76995486068129;0.190955081101934]
Robot Arm 1 togoal: 17.7965388931622

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.794258e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [-5.29817428757726;6.97302255920324;7.88954434834791]
Robot Arm 1 end: [6.81333666727104;8.45771848511098;0.133830277714768]
Robot Arm 1 togoal: 3.80232761683584
Mobile Platform 2 Pose: [-17.7929044779659;-1.65390508341985;5.11163090373895]
Robot Arm 2 end: [-1.92500084477252;5.80909467136482;0.278446801902225]
Robot Arm 1 togoal: 17.7954214374148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.360384e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [-5.19104913099122;6.98582514437221;7.74515432453067]
Robot Arm 1 end: [6.91453841877646;8.29418426807304;0.0871507493072757]
Robot Arm 1 togoal: 3.80602209060957
Mobile Platform 2 Pose: [-17.7013961393877;-1.71640797929274;5.11871763636005]
Robot Arm 2 end: [-2.0003521131128;5.84162615554156;0.393082662284102]
Robot Arm 1 togoal: 17.7826793000053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.031032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [-4.63749040110566;6.76269426443785;7.63240411202651]
Robot Arm 1 end: [6.77567469379297;8.12687102419012;0.0490724083566652]
Robot Arm 1 togoal: 4.00892213385426
Mobile Platform 2 Pose: [-17.5927645282824;-1.79069249956491;5.12572048417586]
Robot Arm 2 end: [-2.09302663521004;5.85752674024909;0.534719395632524]
Robot Arm 1 togoal: 17.7469471047773

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.856927e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [-4.09049618475655;6.52806723722991;7.54608981365381]
Robot Arm 1 end: [6.63647732599855;7.9423916842459;0.0327466156870936]
Robot Arm 1 togoal: 4.21449106008081
Mobile Platform 2 Pose: [-17.463366558909;-1.88377205113897;5.1349163802343]
Robot Arm 2 end: [-2.20351429130729;5.84866488852893;0.683121311847088]
Robot Arm 1 togoal: 17.682403933029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.875108e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [-3.57872406987731;6.29393175719356;7.45845090401451]
Robot Arm 1 end: [6.54707573363323;7.81201614137852;0.107389691226133]
Robot Arm 1 togoal: 4.32530385752543
Mobile Platform 2 Pose: [-17.3208225801778;-1.99490680541333;5.14635246787213]
Robot Arm 2 end: [-2.26544032795333;5.8435754941178;0.826316672794297]
Robot Arm 1 togoal: 17.6444008876289

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.534824e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [-3.43781465678867;6.24337402332734;7.37594126994372]
Robot Arm 1 end: [6.43031498831811;7.79683037899971;0.221748548314364]
Robot Arm 1 togoal: 4.3914304954345
Mobile Platform 2 Pose: [-17.1992759366784;-2.11469426769348;5.1679829318858]
Robot Arm 2 end: [-2.39774234119722;5.79410998685027;0.97208890152298]
Robot Arm 1 togoal: 17.5370902712388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.854340e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [-3.34687764465142;6.20531598231889;7.32548477648275]
Robot Arm 1 end: [6.24734238124406;7.7942422027807;0.34912518113146]
Robot Arm 1 togoal: 4.50789277320334
Mobile Platform 2 Pose: [-17.1879829359967;-2.19184971677482;5.1962296751494]
Robot Arm 2 end: [-2.57935202090927;5.74585493076423;1.01259176347558]
Robot Arm 1 togoal: 17.4142597564531

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.834528e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [-3.25352148760325;6.16283953251096;7.29055062330676]
Robot Arm 1 end: [6.05179510659292;7.73892695986376;0.465956609478876]
Robot Arm 1 togoal: 4.67053420167724
Mobile Platform 2 Pose: [-17.1983403854898;-2.27086328713479;5.21693104995727]
Robot Arm 2 end: [-2.77567193828212;5.68849225201862;0.995108590374183]
Robot Arm 1 togoal: 17.2799382770799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.026039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [-3.08160246617533;6.08031864949154;7.25351184196601]
Robot Arm 1 end: [5.84694784926213;7.60681809062045;0.575928398871504]
Robot Arm 1 togoal: 4.88552168651308
Mobile Platform 2 Pose: [-17.1167423233381;-2.40486172401907;5.22122290726516]
Robot Arm 2 end: [-2.91730711750126;5.58976849800041;1.09168233233358]
Robot Arm 1 togoal: 17.128623235649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.679398e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [-2.86342369402059;5.9686824607925;7.22283907714478]
Robot Arm 1 end: [5.62481984339428;7.52266190891169;0.715403512977693]
Robot Arm 1 togoal: 5.09199284369256
Mobile Platform 2 Pose: [-17.1226871434101;-2.50318086552169;5.21902376064229]
Robot Arm 2 end: [-3.09528883965304;5.50656206915046;1.07193745269565]
Robot Arm 1 togoal: 16.9802874116909

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.790149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [-2.59659487884942;5.81715767370251;7.21478337099118]
Robot Arm 1 end: [5.40684238611226;7.42364849470793;0.829430584879541]
Robot Arm 1 togoal: 5.31158659919668
Mobile Platform 2 Pose: [-17.0090011152331;-2.65908241543059;5.20330939527332]
Robot Arm 2 end: [-3.20485983575441;5.4225227003848;1.15950346822884]
Robot Arm 1 togoal: 16.8570108868124
Step 393:
Mobile Platform 1 Pose: [-2.18744049802028;5.57740916641862;7.21496864588053]
Robot Arm 1 end: [5.10492356867067;7.44180203421104;0.747964700266188]
Robot Arm 1 togoal: 5.57707214184622
Mobile Platform 2 Pose: [-17.0110472525725;-2.77466556221206;5.18345483143692]
Robot Arm 2 end: [-3.3755806918829;5.37112957967474;1.12649373897943]
Robot Arm 1 togoal: 16.7424666914673
Step 394:
Mobile Platform 1 Pose: [-1.81326783586342;5.35482898749861;7.21573177116201]
Robot Arm 1 end: [4.82531372253984;7.44179487153088;0.63714726299783]
Robot Arm 1 togoal: 5.83977629795581
Mobile Platform 2 Pose: [-16.8779011143191;-2.94170751168706;5.15953988403843]
Robot Arm 2 end: [-3.45941888983725;5.36229770459289;1.20692858427222]
Robot Arm 1 togoal: 16.6996416990705
Step 395:
Mobile Platform 1 Pose: [-1.63147190286208;5.2444092282356;7.21877647739274]
Robot Arm 1 end: [4.65953829621709;7.38497032948953;0.557748041339685]
Robot Arm 1 togoal: 6.02384974273802
Mobile Platform 2 Pose: [-16.9160006668328;-3.04261669646974;5.15065984404905]
Robot Arm 2 end: [-3.65034665158936;5.34009320212012;1.13112376482512]
Robot Arm 1 togoal: 16.6068829276739
Step 396:
Mobile Platform 1 Pose: [-1.53035231849152;5.1855314008645;7.21068957692492]
Robot Arm 1 end: [4.55470231499874;7.28096934174476;0.505798803806316]
Robot Arm 1 togoal: 6.17049658192637
Mobile Platform 2 Pose: [-16.7652990032269;-3.20514086450519;5.13587742007324]
Robot Arm 2 end: [-3.72959754675862;5.34077574261627;1.21126568064536]
Robot Arm 1 togoal: 16.5756834748811
Step 397:
Mobile Platform 1 Pose: [-1.46944092189291;5.15774331865078;7.18926953990981]
Robot Arm 1 end: [4.48766194022518;7.16092055944358;0.52865914690628]
Robot Arm 1 togoal: 6.2794094725341
Mobile Platform 2 Pose: [-16.800171311396;-3.30864045626012;5.14109489139398]
Robot Arm 2 end: [-3.87872504615108;5.36717245716939;1.1604956041201]
Robot Arm 1 togoal: 16.5453908931899

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.583316e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [-1.15759563894657;4.98631007114723;7.13218276328753]
Robot Arm 1 end: [4.30098868217593;7.01299381237604;0.516858133090903]
Robot Arm 1 togoal: 6.51223746913995
Mobile Platform 2 Pose: [-16.6982595316181;-3.4610617461813;5.13885688045155]
Robot Arm 2 end: [-3.96298882169422;5.33251113388781;1.23518388079726]
Robot Arm 1 togoal: 16.4806884714134

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.431175e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [-0.710199813635096;4.75173995381743;7.04571392464668]
Robot Arm 1 end: [4.05469737089935;6.84545723707252;0.57353812710238]
Robot Arm 1 togoal: 6.79672328370987
Mobile Platform 2 Pose: [-16.7901249010658;-3.52279252854216;5.14700350055438]
Robot Arm 2 end: [-4.09740686701795;5.38453749821009;1.15683638224399]
Robot Arm 1 togoal: 16.4820270254558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.479208e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',33,1)">33  </a>end
X_d

X_d =

    5.0596
   -4.9081
   -4.8104
    4.6588
    0.0142
    0.8573
    0.0142
    0.0000
    0.0142
   -0.8573
    0.0142

mobile_robot.J*X_d

ans =

         0
   -0.0000
   -0.0019
    0.7260
    0.6872
         0

mobile_robot.car.J*X_d(1:4)

ans =

   -0.0587
    0.0401
    0.6872

mobile_robot.arm.J_body*X_d(5:end)

ans =

         0
   -0.0000
    0.0568
    0.6859
         0
         0

dx

dx =

    5.0596
   -4.9081
   -4.8104
    4.6588
    0.0142
    0.7418
    0.0142
   -0.0000
    0.0142
   -0.8573
    0.0142

mobile_robot.car.J*dx(1:4)

ans =

   -0.0587
    0.0401
    0.6872

mobile_robot.arm.J_body*dx(5:end)

ans =

         0
   -0.1156
    0.0568
    0.8663
         0
         0

if system_dependent('IsDebugMode')==1, dbquit; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.35243439725933;9.99558382645511;9.81851213743633]
Robot Arm 2 end: [9.77353840740595;9.83011984103953;1.48915678296588]
Robot Arm 1 togoal: 28.0040619919614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.16345084730729;9.86594266543014;9.81487686336356]
Robot Arm 2 end: [9.54829515913636;9.88233527904614;1.45852693712879]
Robot Arm 1 togoal: 27.8827366422434

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.886418e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-2.00954779539919;9.74887934028698;9.82884075621975]
Robot Arm 2 end: [9.24944309358791;9.99754053648383;1.34061858958924]
Robot Arm 1 togoal: 27.7574354790715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.868319e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.90339036703045;9.65530830747582;9.85302996757905]
Robot Arm 2 end: [8.99995076135305;9.98167048073853;1.2031782440453]
Robot Arm 1 togoal: 27.5747401583989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.918740e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.75441075625791;9.5385681762822;9.86517275439496]
Robot Arm 2 end: [8.8280290195609;10.0510412445739;1.1233973010661]
Robot Arm 1 togoal: 27.5081264294613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.837990e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.61189355414927;9.46946821915717;9.85800303920606]
Robot Arm 2 end: [8.71428040130406;9.97578341259033;1.06984067696887]
Robot Arm 1 togoal: 27.3762626854016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.884117e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.5235930075504;9.41996023213366;9.85309799306623]
Robot Arm 2 end: [8.67157681606238;9.81562253514349;1.08653170271152]
Robot Arm 1 togoal: 27.2300466388648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.926495e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.4546426570674;9.37941200887306;9.84531518040146]
Robot Arm 2 end: [8.68237221485161;9.6874907866932;1.12091201058752]
Robot Arm 1 togoal: 27.1438463643173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961304e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.3878016085793;9.34085386956679;9.834014453246]
Robot Arm 2 end: [8.67711521887137;9.59567166688202;1.10151378022419]
Robot Arm 1 togoal: 27.0739902796729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.32641702748867;9.30541535686732;9.81926867491138]
Robot Arm 2 end: [8.63753471763505;9.49466126787852;1.0256857094489]
Robot Arm 1 togoal: 26.9748930405071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.26693595297038;9.27363627243209;9.80683551536996]
Robot Arm 2 end: [8.64137112094337;9.34840813549841;0.953583481567181]
Robot Arm 1 togoal: 26.8734734664493

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.1748841646805;9.22780679358013;9.79271833492289]
Robot Arm 2 end: [8.6848634247472;9.19491257094528;0.875605717564833]
Robot Arm 1 togoal: 26.7952481531239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.02469463809637;9.15191740328049;9.76798279349154]
Robot Arm 2 end: [8.74810579811854;9.05347375729676;0.792176316040988]
Robot Arm 1 togoal: 26.7405594032687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.780014273134217;9.0170397816662;9.70371643559844]
Robot Arm 2 end: [8.78460031831939;8.92398595457085;0.690501844854155]
Robot Arm 1 togoal: 26.6771096769028

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.699772906488702;8.89663525813951;9.53166942091573]
Robot Arm 2 end: [8.7620725491179;8.71535017752088;0.718959090504246]
Robot Arm 1 togoal: 26.5126944731406

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.940714207056187;8.98891415996976;9.47693705379981]
Robot Arm 2 end: [8.72580139088804;8.69776936828577;0.861276198452458]
Robot Arm 1 togoal: 26.4706904438649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998901e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-1.06452479094775;9.00974991773208;9.29580077289781]
Robot Arm 2 end: [8.72295012868156;8.53852928860291;0.920847580665433]
Robot Arm 1 togoal: 26.3550016625292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.11792296767681;9.02144357945795;9.12619442170478]
Robot Arm 2 end: [8.70126652456932;8.36107077839695;0.802909861989099]
Robot Arm 1 togoal: 26.2179698527286

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999995e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.26062157682211;9.08361634487845;8.9790705331207]
Robot Arm 2 end: [8.8676727980438;8.20800220756212;0.881419292813689]
Robot Arm 1 togoal: 26.2284098771854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.39933362850357;9.15888800799168;8.84624235795862]
Robot Arm 2 end: [9.00590872464465;8.11385559988866;0.978774132324972]
Robot Arm 1 togoal: 26.2608137732767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.29506445132532;9.12840076949463;8.71120557139023]
Robot Arm 2 end: [8.95632442948555;7.91204533460469;0.82126549665703]
Robot Arm 1 togoal: 26.0897150678196

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.312342e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.49147198630448;9.22978289678154;8.61310363887486]
Robot Arm 2 end: [9.18900192831094;7.882717094551;1.01078273561102]
Robot Arm 1 togoal: 26.2348944603432
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.37833814089677;9.20361646471349;8.491991943679]
Robot Arm 2 end: [9.14147160149921;7.68798927491043;0.861449048316604]
Robot Arm 1 togoal: 26.0709782539554
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.5635213407046;9.30662578282425;8.395725572985]
Robot Arm 2 end: [9.38300220942074;7.68796096313884;1.04722449306586]
Robot Arm 1 togoal: 26.2447937868619
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.4492897767935;9.28694219969704;8.28871111647727]
Robot Arm 2 end: [9.32281608294761;7.49096287149207;0.888270108228677]
Robot Arm 1 togoal: 26.0711593655379
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.62618725287036;9.39100151973414;8.19955886244129]
Robot Arm 2 end: [9.5670623811919;7.52475158088899;1.07152753701287]
Robot Arm 1 togoal: 26.2714459765029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.861104e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-1.50941861379619;9.37388946115504;8.10577477436061]
Robot Arm 2 end: [9.47667840334016;7.31396638161554;0.888604303221643]
Robot Arm 1 togoal: 26.0674961936921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-1.68136310640539;9.47799454476127;8.0216149020061]
Robot Arm 2 end: [9.70935539753576;7.36419224908041;1.07224529443986]
Robot Arm 1 togoal: 26.2711865631372

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990871e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-1.56036963261899;9.45943634689503;7.93412719960679]
Robot Arm 2 end: [9.59739208958319;7.14732617792213;0.887461386102691]
Robot Arm 1 togoal: 26.0478395061896

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000690e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-1.72418698848788;9.56224612245873;7.84551449379523]
Robot Arm 2 end: [9.82636437204139;7.18640303846868;1.05034264811977]
Robot Arm 1 togoal: 26.2426883523109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988245e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-1.59845669944823;9.54084885402061;7.75957377055023]
Robot Arm 2 end: [9.68670344042892;6.98156967493465;0.909731324224737]
Robot Arm 1 togoal: 26.0060310383965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000991e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-1.73902084234615;9.63643062072726;7.65438176980576]
Robot Arm 2 end: [9.90811020974722;6.99730572633137;1.04867428164166]
Robot Arm 1 togoal: 26.1813740216598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002562e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-1.59852316607571;9.6044266988564;7.57432560504]
Robot Arm 2 end: [9.75414520292895;6.78152447952915;0.859926777411997]
Robot Arm 1 togoal: 25.9284175059154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-1.76356298182338;9.70813644253179;7.48094312359665]
Robot Arm 2 end: [9.88810072717203;6.79405934565557;1.05020459808203]
Robot Arm 1 togoal: 26.0345660257668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001227e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-1.61694401248066;9.66766561068366;7.38248182941689]
Robot Arm 2 end: [9.70385588567407;6.57769986120548;0.864233194476093]
Robot Arm 1 togoal: 25.758365866034

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.886689e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-1.80825688086135;9.77153636578372;7.27556907485548]
Robot Arm 2 end: [9.76692488101842;6.56627495991913;1.05136070774136]
Robot Arm 1 togoal: 25.7952194478803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974148e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-1.61620787470174;9.69116209570077;7.16311636644384]
Robot Arm 2 end: [9.51046598555148;6.38008499416094;0.871140810958294]
Robot Arm 1 togoal: 25.4830882028627

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.868819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-1.78816727929545;9.75337743527391;7.00655333390539]
Robot Arm 2 end: [9.49244685690552;6.34117461042461;1.04192707095544]
Robot Arm 1 togoal: 25.4404988746097

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-1.54891063178532;9.63269563566975;6.93159930026704]
Robot Arm 2 end: [9.22673780775533;6.23709048215162;0.86830291438036]
Robot Arm 1 togoal: 25.1741249607474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-1.34747220193235;9.47881873898191;6.76545817539304]
Robot Arm 2 end: [9.03123930524649;6.07084012191799;0.7868214761058]
Robot Arm 1 togoal: 24.9198513202378

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-1.40375652867624;9.39566836459337;6.5781034452186]
Robot Arm 2 end: [8.91702290276287;5.91792614499211;0.892195695819828]
Robot Arm 1 togoal: 24.7311447999084

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [-1.43162464357839;9.30135895147619;6.37972566247705]
Robot Arm 2 end: [8.72785653917224;5.7709268235517;0.77788979115239]
Robot Arm 1 togoal: 24.4950394204241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-1.30042476765215;9.14480805720316;6.19437625045642]
Robot Arm 2 end: [8.61269210562605;5.53280512098187;0.595153119795834]
Robot Arm 1 togoal: 24.260204755748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-1.49520512561408;9.1961177278004;6.10665224828128]
Robot Arm 2 end: [8.612192544716;5.51002809871038;0.772140144520604]
Robot Arm 1 togoal: 24.239131049349

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-1.62045345466617;9.14071391486953;5.91137259742028]
Robot Arm 2 end: [8.59264271988487;5.35198044799735;0.919403758434772]
Robot Arm 1 togoal: 24.1191124426843

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-1.34847484186729;8.9415870231364;5.76021282632305]
Robot Arm 2 end: [8.33490672709599;5.17704903326409;0.723538298901773]
Robot Arm 1 togoal: 23.8148602182362

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.686703e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986270e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.5730723149556;-6.24962959970908;-7.10439671995214]
Robot Arm 1 end: [-6.13857286341445;-6.93245725429985;1.24046798271445]
Robot Arm 1 togoal: 23.3931686812443
Mobile Platform 2 Pose: [-1.5223607196168;8.93076837843576;5.59379150280004]
Robot Arm 2 end: [8.351462507764;5.03165258329133;0.839433999500045]
Robot Arm 1 togoal: 23.7316516022732

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.878968e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986215e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407686765712;-6.21793363477342;-7.10337694299727]
Robot Arm 1 end: [-6.10322676973981;-6.88080904910955;1.17444736021552]
Robot Arm 1 togoal: 23.3322893279315
Mobile Platform 2 Pose: [-1.54948755695241;8.85124681020684;5.39948474949176]
Robot Arm 2 end: [8.38074067384783;4.76884325300118;0.924719146465528]
Robot Arm 1 togoal: 23.5865620556972

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.162418e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984902e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66860125686971;-6.18978163609392;-7.10219045090131]
Robot Arm 1 end: [-6.07860538670947;-6.82838246614364;1.10288087229252]
Robot Arm 1 togoal: 23.2785487351185
Mobile Platform 2 Pose: [-1.63766969810958;8.83057396258604;5.21755311351436]
Robot Arm 2 end: [8.4295465644306;4.52588881587348;0.829852072211112]
Robot Arm 1 togoal: 23.4761009948356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000138e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70849342997397;-6.16354955633444;-7.10107914818559]
Robot Arm 1 end: [-6.05874543805222;-6.77599122083615;1.02316621459629]
Robot Arm 1 togoal: 23.2285390191382
Mobile Platform 2 Pose: [-1.81960639705357;8.86780181448539;5.06216452087504]
Robot Arm 2 end: [8.58950535435926;4.35109327041772;0.907491573226005]
Robot Arm 1 togoal: 23.4925515848258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.7454564596828;-6.13813960106079;-7.10000618125631]
Robot Arm 1 end: [-6.04184117639571;-6.72079346767023;0.931136355095934]
Robot Arm 1 togoal: 23.179161797545
Mobile Platform 2 Pose: [-1.66691676416589;8.75317259463642;4.89200024028879]
Robot Arm 2 end: [8.36216549307997;4.20177084336225;0.825276835454713]
Robot Arm 1 togoal: 23.2237690180624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988426e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78021549060809;-6.11349205173232;-7.09883706297938]
Robot Arm 1 end: [-6.02915086747086;-6.65859173142114;0.819324419361609]
Robot Arm 1 togoal: 23.1286554867934
Mobile Platform 2 Pose: [-1.86662960100258;8.79546286411152;4.74843583321472]
Robot Arm 2 end: [8.52441690752794;4.02366205710191;0.872002633181531]
Robot Arm 1 togoal: 23.2430272735254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973868e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.863023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.8056989382299;-6.0937512412349;-7.09792524014298]
Robot Arm 1 end: [-5.99691300842056;-6.60077739784917;0.729613624741142]
Robot Arm 1 togoal: 23.0675817633071
Mobile Platform 2 Pose: [-1.69946039705454;8.66017987246774;4.5788238839804]
Robot Arm 2 end: [8.27912138065009;3.88601894667595;0.788044738262664]
Robot Arm 1 togoal: 22.9670421267453

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981120e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82792623805884;-6.0758204799901;-7.09846034365323]
Robot Arm 1 end: [-5.98022518808176;-6.56239504830543;0.647709790668639]
Robot Arm 1 togoal: 23.0313517316646
Mobile Platform 2 Pose: [-1.89122740005616;8.68900096642432;4.43907816505364]
Robot Arm 2 end: [8.42441395758548;3.67367869266459;0.701889293103901]
Robot Arm 1 togoal: 22.9586467593552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984193e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85322246270015;-6.05668827226283;-7.10078331223536]
Robot Arm 1 end: [-5.95142598999031;-6.53303560786803;0.574212743452556]
Robot Arm 1 togoal: 22.9931655853284
Mobile Platform 2 Pose: [-1.67944396623091;8.52796976195886;4.28774566044662]
Robot Arm 2 end: [8.15296702309277;3.58594531736013;0.617750880782961]
Robot Arm 1 togoal: 22.6919364726583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984409e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89375654685131;-6.02998165762811;-7.10432606713813]
Robot Arm 1 end: [-5.9177817690024;-6.50862148742725;0.51400350023439]
Robot Arm 1 togoal: 22.954833953032
Mobile Platform 2 Pose: [-1.76640660605673;8.49003292191796;4.12766152624868]
Robot Arm 2 end: [8.24444234252956;3.36930397919384;0.497016806051918]
Robot Arm 1 togoal: 22.6416984093389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94473304157953;-5.99866576866425;-7.10848866523969]
Robot Arm 1 end: [-5.88968285298599;-6.4878926589687;0.465528479424838]
Robot Arm 1 togoal: 22.9226230094397
Mobile Platform 2 Pose: [-1.56547624143382;8.34810011801281;3.98819123214037]
Robot Arm 2 end: [7.98409949643061;3.30907841238767;0.453876710197252]
Robot Arm 1 togoal: 22.3986194887149

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981096e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99849051904003;-5.96720265468653;-7.11353260326854]
Robot Arm 1 end: [-5.86437236568853;-6.47091471327465;0.427808881181114]
Robot Arm 1 togoal: 22.894637120327
Mobile Platform 2 Pose: [-1.52739542630549;8.2549419859403;3.8248847622715]
Robot Arm 2 end: [7.91381997097259;3.14582745853567;0.390128070996656]
Robot Arm 1 togoal: 22.2485189808628

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966238e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983821e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.05157343694889;-5.93795439578981;-7.11973441485354]
Robot Arm 1 end: [-5.81305416402104;-6.46689537212759;0.391564787069883]
Robot Arm 1 togoal: 22.8579734248482
Mobile Platform 2 Pose: [-1.40432112018922;8.13624909391724;3.67255943948946]
Robot Arm 2 end: [7.70812846664223;3.1001371873089;0.320007978069166]
Robot Arm 1 togoal: 22.059777649699

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.745984e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968974e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.08747912100421;-5.92005198310325;-7.12477776395172]
Robot Arm 1 end: [-5.76060387643103;-6.47277580821061;0.375617984376297]
Robot Arm 1 togoal: 22.8267579243978
Mobile Platform 2 Pose: [-1.17339474293812;7.95294126628983;3.5343985072627]
Robot Arm 2 end: [7.40641753055913;3.03814841977446;0.398972462334669]
Robot Arm 1 togoal: 21.7769518358172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.852744e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986446e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.12790127616164;-5.89312882697204;-7.12372550776044]
Robot Arm 1 end: [-5.7774055136222;-6.47438831169658;0.3785516263168]
Robot Arm 1 togoal: 22.8393779090494
Mobile Platform 2 Pose: [-1.0196554189934;7.78448712193933;3.39720848115544]
Robot Arm 2 end: [7.19304438240246;3.02236314973786;0.332510582872535]
Robot Arm 1 togoal: 21.6008153603871

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.855461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942556e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.16135611486631;-5.87162404250542;-7.12395146213762]
Robot Arm 1 end: [-5.81579123736722;-6.47610698074855;0.376313382270834]
Robot Arm 1 togoal: 22.867261776563
Mobile Platform 2 Pose: [-0.695083944970766;7.59646319334307;3.33094434835248]
Robot Arm 2 end: [6.95253235726012;2.99866606729929;0.416794127058214]
Robot Arm 1 togoal: 21.3909547113482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.676776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937449e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.1881695629827;-5.85468681851458;-7.12471201943719]
Robot Arm 1 end: [-5.83142432431782;-6.48252639859799;0.36942235185473]
Robot Arm 1 togoal: 22.8830462066743
Mobile Platform 2 Pose: [-0.479576108307792;7.48450758164496;3.29658642589476]
Robot Arm 2 end: [6.89306900555145;2.89855950314811;0.378287181152326]
Robot Arm 1 togoal: 21.2850748241329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.380236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.21400382845154;-5.8378703587393;-7.1256611011339]
Robot Arm 1 end: [-5.80445651380144;-6.49306866251213;0.359913058044352]
Robot Arm 1 togoal: 22.8724787132869
Mobile Platform 2 Pose: [-0.289025792256839;7.38772646865398;3.2701046356743]
Robot Arm 2 end: [6.89256486455303;2.75095690837152;0.393499943876494]
Robot Arm 1 togoal: 21.1947376996942

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.535861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.24386303976969;-5.81814463719487;-7.127444970481]
Robot Arm 1 end: [-5.82135166114547;-6.50155275621908;0.352017792858985]
Robot Arm 1 togoal: 22.890673453451
Mobile Platform 2 Pose: [-0.047072666049204;7.26029434527646;3.22338515882544]
Robot Arm 2 end: [6.88638431423832;2.60936815568699;0.368375388505875]
Robot Arm 1 togoal: 21.1062712004946

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819535e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000713e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.27970593201636;-5.79554829238145;-7.12932200830265]
Robot Arm 1 end: [-5.86339702556656;-6.51318813793413;0.345641265114309]
Robot Arm 1 togoal: 22.9284586447767
Mobile Platform 2 Pose: [0.175165454881918;7.08045984660322;3.05720821712245]
Robot Arm 2 end: [6.81022215977527;2.41049875881214;0.302605157506059]
Robot Arm 1 togoal: 20.9305646004997

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968528e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.31330779414149;-5.77440832343241;-7.13082762049157]
Robot Arm 1 end: [-5.93495724557374;-6.54229596142743;0.343977697965429]
Robot Arm 1 togoal: 22.9990391015192
Mobile Platform 2 Pose: [-0.155187460148569;7.15993984732478;2.96526991017366]
Robot Arm 2 end: [6.8333202388286;2.42590026146214;0.216951838255935]
Robot Arm 1 togoal: 20.963401660487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997182e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.34100372793989;-5.75714829141054;-7.13199551407892]
Robot Arm 1 end: [-6.01406550162495;-6.58240532647608;0.348416000870285]
Robot Arm 1 togoal: 23.0824914932702
Mobile Platform 2 Pose: [0.0777081857041501;6.92169464089243;2.84152350859958]
Robot Arm 2 end: [6.65872207382498;2.22718038882906;0.281212430821644]
Robot Arm 1 togoal: 20.7015466719421

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970780e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000534e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.36324848785754;-5.74350648684305;-7.13335800744336]
Robot Arm 1 end: [-6.10652204193655;-6.63621136266702;0.367081368060312]
Robot Arm 1 togoal: 23.1843720855204
Mobile Platform 2 Pose: [-0.144152852296865;6.88751674431091;2.74701025849671]
Robot Arm 2 end: [6.56075351100561;2.21319533381321;0.210469826962653]
Robot Arm 1 togoal: 20.6188793693693

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970328e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961676e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.37983907861855;-5.73366564044108;-7.13594690805845]
Robot Arm 1 end: [-6.21294694576369;-6.70792204034187;0.403596076895808]
Robot Arm 1 togoal: 23.308000752953
Mobile Platform 2 Pose: [0.136586941982554;6.65119853306172;2.66544002264611]
Robot Arm 2 end: [6.31657907105176;2.08912458418283;0.266630411777186]
Robot Arm 1 togoal: 20.3457889711271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.39700161116355;-5.72386856361642;-7.14195835074097]
Robot Arm 1 end: [-6.28602643517105;-6.74047369345788;0.468480893998694]
Robot Arm 1 togoal: 23.3791769101682
Mobile Platform 2 Pose: [0.250097669106424;6.42322654425283;2.56316269630772]
Robot Arm 2 end: [5.99375770305109;2.02706551697865;0.316646316724367]
Robot Arm 1 togoal: 20.0474701198935

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960185e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.41579746732865;-5.71334529961931;-7.15244641849258]
Robot Arm 1 end: [-6.34377586679741;-6.78928123311616;0.547166460643241]
Robot Arm 1 togoal: 23.4509557517631
Mobile Platform 2 Pose: [0.41215390701792;6.20285683014237;2.44510636368037]
Robot Arm 2 end: [5.8606747716219;1.86660923770463;0.277366827812614]
Robot Arm 1 togoal: 19.8475198067291

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000535e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.4470815067025;-5.69596183781278;-7.17289813056654]
Robot Arm 1 end: [-6.37640575605969;-6.86041270443687;0.614102066323204]
Robot Arm 1 togoal: 23.5219609283144
Mobile Platform 2 Pose: [0.278361001841068;6.09809494157837;2.37750124508436]
Robot Arm 2 end: [5.70483164654101;1.89320480399805;0.217679704288832]
Robot Arm 1 togoal: 19.7430671717758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000876e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980464e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.50763565208111;-5.66172439186622;-7.20905787581717]
Robot Arm 1 end: [-6.36897017724329;-6.93376080826103;0.656468200583176]
Robot Arm 1 togoal: 23.5678351786302
Mobile Platform 2 Pose: [0.54568604447304;5.85379730881602;2.30990437943897]
Robot Arm 2 end: [5.42748914275766;1.80032933081568;0.270318759577431]
Robot Arm 1 togoal: 19.4632833080845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000063e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.302728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.5921466137806;-5.61327661079773;-7.26150437995722]
Robot Arm 1 end: [-6.34766852061894;-7.06843027876651;0.56282242204162]
Robot Arm 1 togoal: 23.6536610805188
Mobile Platform 2 Pose: [0.581328598925216;5.63732990222003;2.23785369309084]
Robot Arm 2 end: [5.13279214956794;1.79024407400897;0.291287100385036]
Robot Arm 1 togoal: 19.2229926749767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.74916924910241;-5.51938176982246;-7.29204380737677]
Robot Arm 1 end: [-6.24431354042788;-7.14261394453084;0.442377933616295]
Robot Arm 1 togoal: 23.64128509225
Mobile Platform 2 Pose: [0.689661388703067;5.39871378422124;2.16327254110337]
Robot Arm 2 end: [4.90297207577218;1.68347858065522;0.312575818196139]
Robot Arm 1 togoal: 18.9753138956657

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.88396354654971;-5.44152306281049;-7.299119245077]
Robot Arm 1 end: [-6.14157006157109;-7.20303185370938;0.334969705047548]
Robot Arm 1 togoal: 23.6199334646161
Mobile Platform 2 Pose: [0.434672155270015;5.36599950197398;2.14538680494869]
Robot Arm 2 end: [4.83626437328816;1.76061630653727;0.25578697520039]
Robot Arm 1 togoal: 18.9743932831016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.99897006040619;-5.37697570212388;-7.30057933831625]
Robot Arm 1 end: [-6.06087535184142;-7.26306737486405;0.284045920623024]
Robot Arm 1 togoal: 23.6113376929595
Mobile Platform 2 Pose: [0.691063268107287;5.1164584322662;2.1302456512851]
Robot Arm 2 end: [4.47773947686188;1.76475488101548;0.434982566831436]
Robot Arm 1 togoal: 18.6867287569389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-5.09772121862824;-5.32258146537697;-7.30010708098966]
Robot Arm 1 end: [-6.00309023918206;-7.30026215251901;0.271262725814268]
Robot Arm 1 togoal: 23.5999959958423
Mobile Platform 2 Pose: [0.648915100282512;4.90171502799961;2.08017902417782]
Robot Arm 2 end: [4.20418963428884;1.80977469127906;0.510946755734262]
Robot Arm 1 togoal: 18.4999997022126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986902e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-5.18446205611758;-5.27545113467489;-7.29891625525432]
Robot Arm 1 end: [-5.94690974505053;-7.32463824940162;0.257101767400507]
Robot Arm 1 togoal: 23.5805949804449
Mobile Platform 2 Pose: [0.797065708166861;4.65713111684366;2.02842276167774]
Robot Arm 2 end: [3.99777562679794;1.65480414463657;0.73334607872684]
Robot Arm 1 togoal: 18.2316368120834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989486e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-5.25930120764038;-5.23509819927115;-7.29742176023829]
Robot Arm 1 end: [-5.88950494675564;-7.33864017538324;0.241814959038128]
Robot Arm 1 togoal: 23.5529430210623
Mobile Platform 2 Pose: [0.825241421678322;4.49041800744812;1.84212531361143]
Robot Arm 2 end: [3.90336311014742;1.36162036140954;0.832120247126445]
Robot Arm 1 togoal: 17.9684300461482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997389e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-5.30765945402582;-5.20857208900672;-7.29610739302697]
Robot Arm 1 end: [-5.83769295142881;-7.28750762706597;0.225874348006213]
Robot Arm 1 togoal: 23.4812220387708
Mobile Platform 2 Pose: [0.719655103700359;4.40840499137782;1.64917869635076]
Robot Arm 2 end: [3.80571755899216;1.19308852888303;0.749562854425463]
Robot Arm 1 togoal: 17.7898337092748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998862e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-5.34441689694162;-5.18721629122825;-7.29578226880019]
Robot Arm 1 end: [-5.77613883399001;-7.2398484536313;0.207791505717973]
Robot Arm 1 togoal: 23.4056285487691
Mobile Platform 2 Pose: [0.548795325907548;4.35140229072187;1.45615916733658]
Robot Arm 2 end: [3.71090203139808;1.09424174771592;0.829633833341339]
Robot Arm 1 togoal: 17.6507513054759

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-5.36167493385766;-5.17155841168852;-7.29806628078352]
Robot Arm 1 end: [-5.6947321606491;-7.20042431100496;0.165646749310972]
Robot Arm 1 togoal: 23.3241547865572
Mobile Platform 2 Pose: [0.536529333846775;4.17847996282451;1.28166110071722]
Robot Arm 2 end: [3.57387507511448;0.829743069525598;0.754373440099261]
Robot Arm 1 togoal: 17.38163213824

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-5.38784248711146;-5.1529838481239;-7.29863344587744]
Robot Arm 1 end: [-5.58644803765078;-7.11463311703384;0.117222844596009]
Robot Arm 1 togoal: 23.1909167447752
Mobile Platform 2 Pose: [0.476808683810093;4.08951412775442;1.0480265994331]
Robot Arm 2 end: [3.44024640121625;0.674134066545675;0.843691517823281]
Robot Arm 1 togoal: 17.1766151547977

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-5.48114490322542;-5.1061209999947;-7.29320053816627]
Robot Arm 1 end: [-5.52174146041745;-6.97792518085821;0.135604309465255]
Robot Arm 1 togoal: 23.04547076174
Mobile Platform 2 Pose: [0.668158689094745;3.89148855369467;0.895429508968343]
Robot Arm 2 end: [3.2618596450496;0.409955677934943;0.752177457029723]
Robot Arm 1 togoal: 16.8770609575288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-5.56416115972556;-5.07602662626413;-7.27786615214122]
Robot Arm 1 end: [-5.44456057466361;-6.91581095814788;0.106967397497206]
Robot Arm 1 togoal: 22.9494792951211
Mobile Platform 2 Pose: [0.304735939031792;3.9684102377186;0.840364024629435]
Robot Arm 2 end: [3.31580252688557;0.475082718914929;0.84786093885912]
Robot Arm 1 togoal: 16.9555616568357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974378e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-5.53644714070591;-5.11645569539647;-7.24778934844091]
Robot Arm 1 end: [-5.48762670192999;-6.97375400788457;0.0430546212170346]
Robot Arm 1 togoal: 23.025186829235
Mobile Platform 2 Pose: [0.227372460090795;3.81848554344145;0.67541706780711]
Robot Arm 2 end: [3.11868557691722;0.422351708530997;0.81149638042461]
Robot Arm 1 togoal: 16.7698754255518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975727e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-5.48307197138246;-5.18236480217928;-7.20998256220615]
Robot Arm 1 end: [-5.49243557793201;-6.97639943246565;-0.0150469569584759]
Robot Arm 1 togoal: 23.0341732667134
Mobile Platform 2 Pose: [0.549867235136755;3.59123653608042;0.5712158746593]
Robot Arm 2 end: [2.96840229910708;0.142659057502984;0.8457878308136]
Robot Arm 1 togoal: 16.4775271937767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-5.35796728107907;-5.29110326364276;-7.16775058255711]
Robot Arm 1 end: [-5.54350351580841;-6.99862720229397;-0.0645304907358884]
Robot Arm 1 togoal: 23.0882596842112
Mobile Platform 2 Pose: [0.64556259138779;3.40842671870851;0.40068561144025]
Robot Arm 2 end: [2.82637214889288;-0.127511610447121;0.791153463222322]
Robot Arm 1 togoal: 16.2023000055856

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-5.25845723887528;-5.35399058965196;-7.1646603176123]
Robot Arm 1 end: [-5.48914717657626;-7.0816079601898;-0.122152263400078]
Robot Arm 1 togoal: 23.1169842350592
Mobile Platform 2 Pose: [0.277257059739094;3.49399610120084;0.338759620913237]
Robot Arm 2 end: [2.88567644985587;-0.0748270119623036;0.87913921131477]
Robot Arm 1 togoal: 16.2776442301979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-5.22024634698649;-5.37002204811317;-7.17886108939047]
Robot Arm 1 end: [-5.40791974163835;-7.15446912243335;-0.150621325993908]
Robot Arm 1 togoal: 23.1186963191008
Mobile Platform 2 Pose: [-0.0427241979594988;3.50557868241585;0.206113294479933]
Robot Arm 2 end: [2.80009059741596;-0.0466886792204973;0.787235318792603]
Robot Arm 1 togoal: 16.2311162942603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-5.25028307163721;-5.33613950716713;-7.2195844213741]
Robot Arm 1 end: [-5.35410794348216;-7.0934426861444;-0.158988640448689]
Robot Arm 1 togoal: 23.0381591156204
Mobile Platform 2 Pose: [0.240512746078307;3.28058445903041;0.0971288944807377]
Robot Arm 2 end: [2.66522853869452;-0.323031098995537;0.882429256093862]
Robot Arm 1 togoal: 15.9517871614231

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006144e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-5.40100925238649;-5.25141955444063;-7.18707289837682]
Robot Arm 1 end: [-5.2502967186725;-7.05543958537491;-0.164658019949713]
Robot Arm 1 togoal: 22.9412513263368
Mobile Platform 2 Pose: [0.287636443176837;3.11207748200757;-0.0875475758531488]
Robot Arm 2 end: [2.53882948233589;-0.601905160485818;0.823293507259737]
Robot Arm 1 togoal: 15.6854463038069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-5.51886624658098;-5.17799653157162;-7.18055706423054]
Robot Arm 1 end: [-5.1236229955875;-7.07750692500797;-0.158822942679168]
Robot Arm 1 togoal: 22.8732818022493
Mobile Platform 2 Pose: [-0.11825550568454;3.21939985703496;-0.152683924295388]
Robot Arm 2 end: [2.62227747567612;-0.550646359673619;0.905986918496718]
Robot Arm 1 togoal: 15.7794300905746

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-5.61602476318125;-5.11473583821646;-7.17792804852959]
Robot Arm 1 end: [-5.01938556292453;-7.05127009810073;-0.139575068699095]
Robot Arm 1 togoal: 22.7834427645375
Mobile Platform 2 Pose: [-0.465006517204624;3.24630287513506;-0.282658855533702]
Robot Arm 2 end: [2.54543215186953;-0.529293450058628;0.816919577165778]
Robot Arm 1 togoal: 15.734607706393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-5.70267356976416;-5.06038374655456;-7.17970506867398]
Robot Arm 1 end: [-4.89527026054496;-6.98992298093659;-0.115422385254848]
Robot Arm 1 togoal: 22.6540159189446
Mobile Platform 2 Pose: [-0.200878007948758;3.0236636998618;-0.396376917571547]
Robot Arm 2 end: [2.41408269759693;-0.807085749595627;0.911433880643888]
Robot Arm 1 togoal: 15.4593238044854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-5.79833967513084;-5.00489775668096;-7.18337256949144]
Robot Arm 1 end: [-4.69308500918889;-6.86375833829754;-0.0369980491603817]
Robot Arm 1 togoal: 22.4209823669914
Mobile Platform 2 Pose: [-0.22271641006434;2.87077809464832;-0.586125501854789]
Robot Arm 2 end: [2.27773519264344;-1.03672309044049;0.849374153549366]
Robot Arm 1 togoal: 15.2162477534949

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-5.93293911858261;-4.92570578370298;-7.18558672153486]
Robot Arm 1 end: [-4.52366106580997;-6.80067790076042;0.0539731256016838]
Robot Arm 1 togoal: 22.2564974003061
Mobile Platform 2 Pose: [-0.560051542071661;2.90222060697899;-0.718020961564581]
Robot Arm 2 end: [2.28783134437281;-1.0608903865945;0.929449370152855]
Robot Arm 1 togoal: 15.2068541117452

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-6.11669251367262;-4.80393909771558;-7.17697543114798]
Robot Arm 1 end: [-4.42446064937717;-6.70224092493718;0.126410955466091]
Robot Arm 1 togoal: 22.112774487685
Mobile Platform 2 Pose: [-0.565686906824242;2.7491264087345;-0.915662471537076]
Robot Arm 2 end: [2.16247761929204;-1.34846410510518;0.867176549247765]
Robot Arm 1 togoal: 14.9399605181937

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.790136e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-6.33139296348466;-4.66534565216221;-7.16814373593964]
Robot Arm 1 end: [-4.28874002011233;-6.60065118914321;0.246420248833685]
Robot Arm 1 togoal: 21.9402430435044
Mobile Platform 2 Pose: [-0.689521041261676;2.69634609380387;-1.13746948345088]
Robot Arm 2 end: [2.08340560080151;-1.4828998371333;0.949998995204852]
Robot Arm 1 togoal: 14.7944492376453

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.879053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.269873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-6.309272543233;-4.62554649879584;-7.10394198965602]
Robot Arm 1 end: [-4.15241529264589;-6.6002283484897;0.227992412526143]
Robot Arm 1 togoal: 21.8524668735313
Mobile Platform 2 Pose: [-0.435230300616975;2.48921129298069;-1.29288909826962]
Robot Arm 2 end: [1.94832444199919;-1.71263084070233;1.04511372521027]
Robot Arm 1 togoal: 14.5488629212144

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.152396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.122126e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-6.20166464774461;-4.63642383846335;-7.05998822011331]
Robot Arm 1 end: [-4.10521447194165;-6.69806846327348;0.151207357118412]
Robot Arm 1 togoal: 21.9011163548376
Mobile Platform 2 Pose: [-0.64405796289478;2.46150734396768;-1.45348595727109]
Robot Arm 2 end: [1.93852715451405;-1.76820124465912;1.13350062084378]
Robot Arm 1 togoal: 14.5065880597868

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.095374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.325267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-6.08449567122;-4.60925734143058;-7.0015079187618]
Robot Arm 1 end: [-4.02891590911809;-6.73227993704216;0.139635102759798]
Robot Arm 1 togoal: 21.8789360448806
Mobile Platform 2 Pose: [-0.526821550586662;2.36832175284242;-1.58787828499035]
Robot Arm 2 end: [1.92479764062032;-1.94383392102925;1.21384498518504]
Robot Arm 1 togoal: 14.3943375972925

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.833394e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-5.94698490443623;-4.58000967012022;-6.94791117204733]
Robot Arm 1 end: [-3.93106918523394;-6.79435504029897;0.197881169578785]
Robot Arm 1 togoal: 21.8603680962331
Mobile Platform 2 Pose: [-0.565042930239159;2.3058451910309;-1.72658587993269]
Robot Arm 2 end: [1.92493776030916;-2.06647539521227;1.2910557386744]
Robot Arm 1 togoal: 14.3247278370424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.212703e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-5.96358291451352;-4.42690352812575;-6.8939573968231]
Robot Arm 1 end: [-3.78325171805017;-6.80920614344633;0.153700127405666]
Robot Arm 1 togoal: 21.7806553629041
Mobile Platform 2 Pose: [-0.571547453968482;2.23775100809204;-1.84732152752261]
Robot Arm 2 end: [1.85225080329702;-2.27034480270969;1.26391277088275]
Robot Arm 1 togoal: 14.1523677318188

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.814540e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-5.88362470763095;-4.33832369301135;-6.85391399031047]
Robot Arm 1 end: [-3.65957583915762;-6.62429435899616;0.202645932389324]
Robot Arm 1 togoal: 21.5566516993673
Mobile Platform 2 Pose: [-0.52345351594193;2.07212127420665;-1.98894270958909]
Robot Arm 2 end: [1.79250642066081;-2.45208335440744;1.30651343477475]
Robot Arm 1 togoal: 14.0028660586611

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.112008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-5.7665198137998;-4.34921429685939;-6.83789246748996]
Robot Arm 1 end: [-3.70160696538398;-6.56689191307711;0.221001130115374]
Robot Arm 1 togoal: 21.5380114717014
Mobile Platform 2 Pose: [-0.696279699927756;1.98255523940175;-2.11891077504565]
Robot Arm 2 end: [1.73322841061085;-2.68040583625515;1.2379096065663]
Robot Arm 1 togoal: 13.8320370093616

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.333681e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-5.63775900049965;-4.32004330042167;-6.79934857825768]
Robot Arm 1 end: [-3.58691604106203;-6.66905347967598;0.348858977845643]
Robot Arm 1 togoal: 21.536841597388
Mobile Platform 2 Pose: [-1.00142242394736;1.97405594518118;-2.22803917754572]
Robot Arm 2 end: [1.56526555795865;-2.8358129705844;1.10742254716135]
Robot Arm 1 togoal: 13.610730675759

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-5.73797067530612;-4.15295835212192;-6.77291732108431]
Robot Arm 1 end: [-3.50000037320099;-6.60639973506209;0.177142856652802]
Robot Arm 1 togoal: 21.4436115022359
Mobile Platform 2 Pose: [-1.03606795419613;1.75128496828879;-2.34149882612222]
Robot Arm 2 end: [1.40068682100448;-3.0436217382319;1.02743197881785]
Robot Arm 1 togoal: 13.3645227338262

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.250166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-5.55960133851425;-4.14687962199441;-6.73329669099364]
Robot Arm 1 end: [-3.51111706500869;-6.56397223702491;0.205889704718279]
Robot Arm 1 togoal: 21.4159981252814
Mobile Platform 2 Pose: [-0.988817952818821;1.52599116933067;-2.44986258459836]
Robot Arm 2 end: [1.31439628773455;-3.15705941437352;1.11840076296475]
Robot Arm 1 togoal: 13.2288882888917

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-5.62669950664037;-3.99008267375519;-6.69403144258679]
Robot Arm 1 end: [-3.38771713842853;-6.41928293532986;0.20884833801313]
Robot Arm 1 togoal: 21.2260586137192
Mobile Platform 2 Pose: [-1.16444807266975;1.31294187481782;-2.50255162063349]
Robot Arm 2 end: [0.956954291927055;-3.2054411068354;1.02133294294278]
Robot Arm 1 togoal: 12.9023464949585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.798724e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-5.65620190739025;-3.84692206455321;-6.63520660411748]
Robot Arm 1 end: [-3.28719247024141;-6.34668766157726;0.151501082903782]
Robot Arm 1 togoal: 21.1101683681189
Mobile Platform 2 Pose: [-0.950356771628014;1.2034282209551;-2.51823237655889]
Robot Arm 2 end: [0.950735977600192;-3.21303286508704;1.08133152270359]
Robot Arm 1 togoal: 12.8908824256303

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.805254e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-5.55373678211124;-3.78067993776343;-6.56944536412655]
Robot Arm 1 end: [-3.17208003353838;-6.38056297792527;0.18994827470412]
Robot Arm 1 togoal: 21.0617718765333
Mobile Platform 2 Pose: [-0.775908920898304;1.10192904813562;-2.53797047376839]
Robot Arm 2 end: [0.792530199784792;-3.1045179076114;1.18427943015642]
Robot Arm 1 togoal: 12.811704107646

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.944649e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-5.67264882350119;-3.60098394088422;-6.52467873411154]
Robot Arm 1 end: [-3.0517394791343;-6.21928236929454;0.150015217030451]
Robot Arm 1 togoal: 20.8636675414759
Mobile Platform 2 Pose: [-0.405934152962905;0.858483261043429;-2.59484005042384]
Robot Arm 2 end: [0.587795832354968;-2.98691709653693;1.31784001347641]
Robot Arm 1 togoal: 12.7014182821685

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.748731e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-5.56011945325413;-3.54562603736582;-6.45754132932402]
Robot Arm 1 end: [-2.97359675400133;-6.28346052771604;0.138829043037086]
Robot Arm 1 togoal: 20.8656582941299
Mobile Platform 2 Pose: [-0.670784103170821;0.789191584264643;-2.69706761152485]
Robot Arm 2 end: [0.585734958658595;-3.04578198372886;1.38157563873631]
Robot Arm 1 togoal: 12.6664269620643

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.849334e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-5.69190137505399;-3.34241467668458;-6.39878052129854]
Robot Arm 1 end: [-2.79773637841155;-6.09974485809558;0.141199436180566]
Robot Arm 1 togoal: 20.6128040427063
Mobile Platform 2 Pose: [-0.719855454129282;0.559976963554061;-2.79687070592831]
Robot Arm 2 end: [0.433683663628142;-2.93909845731292;1.44651914300998]
Robot Arm 1 togoal: 12.598556773942

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.691030e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.488997e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-5.58210856570324;-3.28088807281225;-6.32684395750161]
Robot Arm 1 end: [-2.73885701905738;-6.16533468905111;0.10082398556765]
Robot Arm 1 togoal: 20.6304004729185
Mobile Platform 2 Pose: [-0.117171311435814;0.208670512822793;-2.84861409758819]
Robot Arm 2 end: [0.116413455627284;-2.90695354551887;1.4772674379553]
Robot Arm 1 togoal: 12.355364897388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.319952e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-5.72768464013021;-3.06253026717628;-6.26042109241828]
Robot Arm 1 end: [-2.54105711969746;-5.97619057174606;0.107045625068079]
Robot Arm 1 togoal: 20.3596671347704
Mobile Platform 2 Pose: [-0.250647681705989;-0.0381017815543415;-2.88865700866924]
Robot Arm 2 end: [-0.200706302287265;-3.01065326758514;1.44328753869637]
Robot Arm 1 togoal: 12.036742248075

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.542807e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-5.62305166268226;-2.99118855254296;-6.1815486317695]
Robot Arm 1 end: [-2.41015537069211;-6.01117293924492;0.141379602981731]
Robot Arm 1 togoal: 20.3045060923677
Mobile Platform 2 Pose: [0.0771287665629823;-0.198131127966669;-2.91147644597]
Robot Arm 2 end: [-0.28944246275761;-2.96746807400644;1.47779089779861]
Robot Arm 1 togoal: 11.9897164270356
Step 123:
Mobile Platform 1 Pose: [-5.73660518067411;-2.80202210112635;-6.12663920095606]
Robot Arm 1 end: [-2.2309668050631;-5.86491785405431;0.143101941086952]
Robot Arm 1 togoal: 20.0796108170254
Mobile Platform 2 Pose: [0.759547264233821;-0.390512535410766;-2.87171473938864]
Robot Arm 2 end: [-0.394026361177404;-2.85928284490703;1.49421647386806]
Robot Arm 1 togoal: 11.9693478684541
Step 124:
Mobile Platform 1 Pose: [-5.65650972876129;-2.71401891577559;-6.05411390905807]
Robot Arm 1 end: [-2.07451679690727;-5.9077430974753;0.201590588273724]
Robot Arm 1 togoal: 20.014771733143
Mobile Platform 2 Pose: [1.26344290963545;-0.562385246984939;-2.89906225326338]
Robot Arm 2 end: [-0.684731824434289;-2.92813300874122;1.49575774457345]
Robot Arm 1 togoal: 11.6955615981543
Step 125:
Mobile Platform 1 Pose: [-5.76076511663972;-2.53549940207498;-6.01023314916904]
Robot Arm 1 end: [-1.93924646172478;-5.80055542945802;0.140477065065082]
Robot Arm 1 togoal: 19.8521787602032
Mobile Platform 2 Pose: [1.70349896927726;-0.73689483317945;-2.92754457372841]
Robot Arm 2 end: [-0.995957729366551;-2.99557388149664;1.47130111121862]
Robot Arm 1 togoal: 11.4077707042381
Step 126:
Mobile Platform 1 Pose: [-5.68057653471589;-2.45099128053941;-5.95016603722625]
Robot Arm 1 end: [-1.79783092612466;-5.86192394496815;0.199313848296266]
Robot Arm 1 togoal: 19.812528866376
Mobile Platform 2 Pose: [2.12147956452013;-0.924115444811746;-2.96767131183812]
Robot Arm 2 end: [-1.26449299902443;-3.04177174115246;1.44849548303846]
Robot Arm 1 togoal: 11.1683158968261
Step 127:
Mobile Platform 1 Pose: [-5.78163356850292;-2.27810261260805;-5.91678284604422]
Robot Arm 1 end: [-1.65791454410539;-5.7788852743108;0.138565501331826]
Robot Arm 1 togoal: 19.6670108852523
Mobile Platform 2 Pose: [2.54179967858312;-1.13206050982675;-3.01621740670112]
Robot Arm 2 end: [-1.54404370518382;-3.2090050736609;1.39283994515111]
Robot Arm 1 togoal: 10.8460699771421
Step 128:
Mobile Platform 1 Pose: [-5.70956854562316;-2.19420718726477;-5.87107047970373]
Robot Arm 1 end: [-1.53206001139068;-5.86098702886571;0.197187897518455]
Robot Arm 1 togoal: 19.6548161049357
Mobile Platform 2 Pose: [2.79959326473786;-1.24988461203221;-3.03868795161066]
Robot Arm 2 end: [-1.58967267608276;-3.15811155295872;1.4421889042702]
Robot Arm 1 togoal: 10.8421056260302
Step 129:
Mobile Platform 1 Pose: [-5.80611247055335;-2.03010163825311;-5.84984957308467]
Robot Arm 1 end: [-1.39379522211607;-5.80829307097793;0.137325315538246]
Robot Arm 1 togoal: 19.5354870687645
Mobile Platform 2 Pose: [3.06886968899623;-1.38054990921583;-3.07036531146734]
Robot Arm 2 end: [-1.69042273841736;-3.11666048848296;1.45974031729312]
Robot Arm 1 togoal: 10.7904212079857
Step 130:
Mobile Platform 1 Pose: [-5.75089545556163;-1.94445486609102;-5.82013414047298]
Robot Arm 1 end: [-1.28528583970122;-5.91192049015515;0.195150332451435]
Robot Arm 1 togoal: 19.5526153623938
Mobile Platform 2 Pose: [3.3312614051424;-1.51665151833181;-3.10082467875329]
Robot Arm 2 end: [-1.81075115448097;-3.13166665684163;1.46333147116229]
Robot Arm 1 togoal: 10.6883640107265
Step 131:
Mobile Platform 1 Pose: [-5.83728417881997;-1.79470675314951;-5.81190040185261]
Robot Arm 1 end: [-1.16228886684461;-5.89864489798765;0.1369814894742]
Robot Arm 1 togoal: 19.4750894626102
Mobile Platform 2 Pose: [3.58410305561966;-1.66080846166341;-3.12271051184798]
Robot Arm 2 end: [-1.91701912821402;-3.16660297861252;1.47569664331164]
Robot Arm 1 togoal: 10.5845144914489
Step 132:
Mobile Platform 1 Pose: [-5.80579534957739;-1.70843923888787;-5.79982867095351]
Robot Arm 1 end: [-1.07833366628093;-6.0229449809992;0.193517818515647]
Robot Arm 1 togoal: 19.5250211684326
Mobile Platform 2 Pose: [3.89821350700216;-1.85280520289419;-3.14222046163376]
Robot Arm 2 end: [-2.02277291896066;-3.21834452005301;1.49675878647455]
Robot Arm 1 togoal: 10.4703195074265
Step 133:
Mobile Platform 1 Pose: [-5.87575755511165;-1.58067948138183;-5.80478215066774]
Robot Arm 1 end: [-0.992951237935823;-6.05090222855441;0.137574091919861]
Robot Arm 1 togoal: 19.5035972711035
Mobile Platform 2 Pose: [4.28084985263034;-1.99510840399425;-3.34776802050199]
Robot Arm 2 end: [-2.10052491696552;-3.34661259422695;1.51246467925968]
Robot Arm 1 togoal: 10.3280851763762
Step 134:
Mobile Platform 1 Pose: [-5.85995327864857;-1.50582967070862;-5.80922617232065]
Robot Arm 1 end: [-0.953985276701397;-6.18763196944168;0.193157702514712]
Robot Arm 1 togoal: 19.5906198826702
Mobile Platform 2 Pose: [4.48412700809022;-2.11627450558059;-3.35895967029285]
Robot Arm 2 end: [-2.26881453918462;-3.32744647185484;1.49976800917933]
Robot Arm 1 togoal: 10.2124752148008
Step 135:
Mobile Platform 1 Pose: [-5.90769382262697;-1.40809232217061;-5.82534174998617]
Robot Arm 1 end: [-0.93132491445981;-6.24609346889765;0.143412389977659]
Robot Arm 1 togoal: 19.6297520363589
Mobile Platform 2 Pose: [4.67326475134109;-2.24273835433415;-3.33546315275691]
Robot Arm 2 end: [-2.39682489480664;-3.27242370055024;1.49455490430744]
Robot Arm 1 togoal: 10.1523028859583
Step 136:
Mobile Platform 1 Pose: [-5.88603678231211;-1.36127634270993;-5.83990824113908]
Robot Arm 1 end: [-0.961129060292914;-6.37385903460573;0.199347313391377]
Robot Arm 1 togoal: 19.7483258983023
Mobile Platform 2 Pose: [4.83042748262935;-2.34244664491992;-3.30141064328446]
Robot Arm 2 end: [-2.48399829456386;-3.24663892074947;1.4969134299425]
Robot Arm 1 togoal: 10.1043924936491
Step 137:
Mobile Platform 1 Pose: [-5.89681128939012;-1.30310956812147;-5.86183706574714]
Robot Arm 1 end: [-1.00652089699966;-6.44166645477963;0.183854561944904]
Robot Arm 1 togoal: 19.8307805189011
Mobile Platform 2 Pose: [4.94521572050456;-2.40810002872517;-3.2942654350008]
Robot Arm 2 end: [-2.52671777220917;-3.29839142310956;1.50272370313013]
Robot Arm 1 togoal: 10.0380196651079
Step 138:
Mobile Platform 1 Pose: [-5.85959978529043;-1.28607021357425;-5.87828093722478]
Robot Arm 1 end: [-1.10161880537374;-6.53377258059162;0.251574843189172]
Robot Arm 1 togoal: 19.9555259533618
Mobile Platform 2 Pose: [4.97403113188991;-2.42353750603684;-3.30555437419878]
Robot Arm 2 end: [-2.55672062829699;-3.44749739493244;1.47907379486272]
Robot Arm 1 togoal: 9.91662523247268
Step 139:
Mobile Platform 1 Pose: [-5.82169282822082;-1.26550786155462;-5.90163346920222]
Robot Arm 1 end: [-1.20079793090402;-6.60737825662904;0.335965577017872]
Robot Arm 1 togoal: 20.0665690498239
Mobile Platform 2 Pose: [4.96496240783782;-2.42008807845119;-3.2743916177787]
Robot Arm 2 end: [-2.57307883782277;-3.53604222227021;1.42577094991549]
Robot Arm 1 togoal: 9.84636870493124
Step 140:
Mobile Platform 1 Pose: [-5.77883158181683;-1.23885637399287;-5.93619498148188]
Robot Arm 1 end: [-1.31331360476275;-6.66892635590693;0.437277944274244]
Robot Arm 1 togoal: 20.1747025770499
Mobile Platform 2 Pose: [4.91490831024905;-2.39096420000805;-3.22619914191205]
Robot Arm 2 end: [-2.62791970919391;-3.59506626979597;1.33628392800471]
Robot Arm 1 togoal: 9.7675413451723
Step 141:
Mobile Platform 1 Pose: [-5.80131753193437;-1.17312849581356;-5.99509041751342]
Robot Arm 1 end: [-1.37400018792944;-6.70364043889297;0.458459806208681]
Robot Arm 1 togoal: 20.2363157677049
Mobile Platform 2 Pose: [4.77565998984458;-2.31380779773521;-3.1382619043179]
Robot Arm 2 end: [-2.71389064263474;-3.52908476290095;1.21078522868388]
Robot Arm 1 togoal: 9.74968547069848
Step 142:
Mobile Platform 1 Pose: [-5.7305439645836;-1.15933758051399;-6.04589120501306]
Robot Arm 1 end: [-1.50443316263584;-6.71704986688303;0.51596984825995]
Robot Arm 1 togoal: 20.3180172322822
Mobile Platform 2 Pose: [4.59926392459054;-2.2485793989812;-3.05125203551985]
Robot Arm 2 end: [-2.74332442235229;-3.43019980260894;1.10162913061153]
Robot Arm 1 togoal: 9.79783072924614
Step 143:
Mobile Platform 1 Pose: [-5.81082734612274;-1.0967311976087;-6.1238196319741]
Robot Arm 1 end: [-1.55019976010563;-6.72428590875712;0.509224898053171]
Robot Arm 1 togoal: 20.3502467432981
Mobile Platform 2 Pose: [4.45666989025912;-2.20569848001907;-2.98622769907276]
Robot Arm 2 end: [-2.814282604818;-3.33716505938386;0.93516047986351]
Robot Arm 1 togoal: 9.81687892726961
Step 144:
Mobile Platform 1 Pose: [-5.77817953371983;-1.09121217959551;-6.21006016240459]
Robot Arm 1 end: [-1.64205722889953;-6.69790793538246;0.43142705991171]
Robot Arm 1 togoal: 20.3849158725577
Mobile Platform 2 Pose: [4.27644472102401;-2.16460341722516;-2.90818128642432]
Robot Arm 2 end: [-2.8514945816839;-3.18161883157622;0.827090503003275]
Robot Arm 1 togoal: 9.90317938157403
Step 145:
Mobile Platform 1 Pose: [-5.59376105353396;-1.14529636711214;-6.25496711472793]
Robot Arm 1 end: [-1.78060446598448;-6.64955610774363;0.513380264515452]
Robot Arm 1 togoal: 20.420716385578
Mobile Platform 2 Pose: [4.12576671801141;-2.18141363270115;-2.81340762607563]
Robot Arm 2 end: [-2.92645594595004;-2.96752241849516;0.619155944883866]
Robot Arm 1 togoal: 10.0151929147022
Step 146:
Mobile Platform 1 Pose: [-5.34479403428568;-1.24491703084543;-6.34852957119715]
Robot Arm 1 end: [-1.93317193036202;-6.60878583128759;0.431432836390819]
Robot Arm 1 togoal: 20.480222550382
Mobile Platform 2 Pose: [4.29102649761219;-2.37146323316608;-2.73843279069799]
Robot Arm 2 end: [-3.05897438668373;-3.0568215273395;0.549497154071762]
Robot Arm 1 togoal: 9.86556544996663
Step 147:
Mobile Platform 1 Pose: [-5.01261641572635;-1.35303881019153;-6.43895368601114]
Robot Arm 1 end: [-2.09518060283562;-6.58699919279711;0.507791755013634]
Robot Arm 1 togoal: 20.5535526609093
Mobile Platform 2 Pose: [4.50840472688325;-2.61730202031956;-2.69300933055406]
Robot Arm 2 end: [-3.25328136964269;-3.11108778614269;0.637993339555562]
Robot Arm 1 togoal: 9.68271782764695
Step 148:
Mobile Platform 1 Pose: [-4.70063873425365;-1.57469354142272;-6.49643668869441]
Robot Arm 1 end: [-2.3052885462092;-6.58747853263058;0.433010891267204]
Robot Arm 1 togoal: 20.6820765537861
Mobile Platform 2 Pose: [4.61994326737391;-2.85931665007858;-2.630328240904]
Robot Arm 2 end: [-3.38026115174292;-3.18096515365319;0.553350773177453]
Robot Arm 1 togoal: 9.55282804623774
Step 149:
Mobile Platform 1 Pose: [-4.1898123802228;-1.77878394946406;-6.64082195575975]
Robot Arm 1 end: [-2.53957950162645;-6.7515391583186;0.50240222366198]
Robot Arm 1 togoal: 20.9497651461837
Mobile Platform 2 Pose: [4.79691185627225;-3.10924028600629;-2.63650693058638]
Robot Arm 2 end: [-3.54640525776407;-3.2597237758892;0.649901950355608]
Robot Arm 1 togoal: 9.37224735516636
Step 150:
Mobile Platform 1 Pose: [-3.99473461547065;-1.93911549147362;-6.66337955131466]
Robot Arm 1 end: [-2.68065262376183;-6.76454241124521;0.42423113766161]
Robot Arm 1 togoal: 21.0487941497928
Mobile Platform 2 Pose: [4.90625126003298;-3.35921918684824;-2.63975487362965]
Robot Arm 2 end: [-3.62723249361056;-3.3473261317919;0.733998234454606]
Robot Arm 1 togoal: 9.2460590018777
Step 151:
Mobile Platform 1 Pose: [-3.65941421701935;-2.16816623326135;-6.70997288831423]
Robot Arm 1 end: [-2.90631501733691;-6.8332575486105;0.384136661126139]
Robot Arm 1 togoal: 21.2420334543533
Mobile Platform 2 Pose: [4.88497572986774;-3.60202345015162;-2.66530672917588]
Robot Arm 2 end: [-3.78175814387032;-3.40516275397783;0.645551261045877]
Robot Arm 1 togoal: 9.10630663756612
Step 152:
Mobile Platform 1 Pose: [-3.42732979738653;-2.33958301024707;-6.74349247977778]
Robot Arm 1 end: [-3.04249892678951;-6.8539833016974;0.311620569581231]
Robot Arm 1 togoal: 21.3454006754598
Mobile Platform 2 Pose: [4.55618192898202;-3.60504525387006;-2.70038010044771]
Robot Arm 2 end: [-3.90797008718479;-3.40964041019138;0.527713688507212]
Robot Arm 1 togoal: 9.02952300391425
Step 153:
Mobile Platform 1 Pose: [-3.00454205397285;-2.52728473576938;-6.84238785804737]
Robot Arm 1 end: [-3.27228482776982;-6.97963208630989;0.382967581599555]
Robot Arm 1 togoal: 21.581394026365
Mobile Platform 2 Pose: [4.25805363800253;-3.53656440912268;-2.72042136974891]
Robot Arm 2 end: [-4.00400486322442;-3.36046104357065;0.52945252648406]
Robot Arm 1 togoal: 9.00103336436518
Step 154:
Mobile Platform 1 Pose: [-2.84259195911342;-2.66349203767248;-6.86542682926475]
Robot Arm 1 end: [-3.37600828866299;-6.98647768399943;0.306368432261129]
Robot Arm 1 togoal: 21.6548690065939
Mobile Platform 2 Pose: [3.93636447575341;-3.48415974883919;-2.75284229136534]
Robot Arm 2 end: [-4.06539037597623;-3.33998884245881;0.652071346011011]
Robot Arm 1 togoal: 8.96272151825838
Step 155:
Mobile Platform 1 Pose: [-2.54427092897696;-2.85621679215811;-6.91196048890874]
Robot Arm 1 end: [-3.54679597309378;-7.05391987377756;0.258806558518777]
Robot Arm 1 togoal: 21.8161636517875
Mobile Platform 2 Pose: [4.03151358660392;-3.7044124599321;-2.87111630903713]
Robot Arm 2 end: [-4.18481618077975;-3.5246170983828;0.550697449626776]
Robot Arm 1 togoal: 8.75719324973123
Step 156:
Mobile Platform 1 Pose: [-2.2814569906212;-3.01281154289918;-6.95845233466137]
Robot Arm 1 end: [-3.66176702832613;-7.0830508471111;0.213618992582016]
Robot Arm 1 togoal: 21.9130953924771
Mobile Platform 2 Pose: [4.02536610531172;-3.90291177207975;-3.02309664683665]
Robot Arm 2 end: [-4.44437472861883;-3.57756112652953;0.466453592398455]
Robot Arm 1 togoal: 8.55714680049717
Step 157:
Mobile Platform 1 Pose: [-2.20494936698292;-2.93310281615501;-7.0278806442638]
Robot Arm 1 end: [-3.61310287540025;-7.15413204676473;0.282504267230969]
Robot Arm 1 togoal: 21.9343267016373
Mobile Platform 2 Pose: [4.27802743645302;-4.05508541330554;-3.04748174635396]
Robot Arm 2 end: [-4.48035755628353;-3.69716141912357;0.611594556464817]
Robot Arm 1 togoal: 8.4272916849835
Step 158:
Mobile Platform 1 Pose: [-2.07474652768711;-2.89140329203355;-7.10074675184117]
Robot Arm 1 end: [-3.52329618291314;-7.20848696659657;0.222828800753891]
Robot Arm 1 togoal: 21.9247990137394
Mobile Platform 2 Pose: [4.59034795245626;-4.30152170640686;-3.08954305214308]
Robot Arm 2 end: [-4.60970899314351;-3.78141861361266;0.722932428547392]
Robot Arm 1 togoal: 8.26818622470039
Step 159:
Mobile Platform 1 Pose: [-2.18518459134303;-2.79308933584622;-7.10173809484999]
Robot Arm 1 end: [-3.46431031944362;-7.21961496343895;0.29957235829776]
Robot Arm 1 togoal: 21.8927996690985
Mobile Platform 2 Pose: [4.69140216715483;-4.55087884792639;-3.10745997087841]
Robot Arm 2 end: [-4.6757089614272;-3.8124325559277;0.900387918624438]
Robot Arm 1 togoal: 8.18652705937746
Step 160:
Mobile Platform 1 Pose: [-2.3278853780018;-2.64520784740487;-7.11286111617229]
Robot Arm 1 end: [-3.36126138492592;-7.23006867800018;0.399414779880709]
Robot Arm 1 togoal: 21.8324649513266
Mobile Platform 2 Pose: [4.46790644668415;-4.61120633264102;-3.06729635375432]
Robot Arm 2 end: [-4.90595118491359;-3.76716003894307;0.699374415889966]
Robot Arm 1 togoal: 8.09152000004299
Step 161:
Mobile Platform 1 Pose: [-2.11320068464468;-2.59361719906237;-7.2517227053689]
Robot Arm 1 end: [-3.34206561967527;-7.38983346218756;0.468784130597036]
Robot Arm 1 togoal: 21.9436592453985
Mobile Platform 2 Pose: [4.6032848164217;-4.85898746021268;-3.10053056553848]
Robot Arm 2 end: [-4.96054938133283;-3.81152158898624;0.895046082927385]
Robot Arm 1 togoal: 8.00531984919196
Step 162:
Mobile Platform 1 Pose: [-1.91840643555189;-2.7446303255868;-7.27601507751706]
Robot Arm 1 end: [-3.46941321411783;-7.4026588895383;0.392630844981362]
Robot Arm 1 togoal: 22.0352183040758
Mobile Platform 2 Pose: [4.43779502424602;-4.93619404226447;-3.08871118570686]
Robot Arm 2 end: [-5.23361314690357;-3.78802228727043;0.689650349924523]
Robot Arm 1 togoal: 7.87389693719063
Step 163:
Mobile Platform 1 Pose: [-1.50314401738392;-2.98206743023265;-7.35426864828169]
Robot Arm 1 end: [-3.71931238432219;-7.50757098337316;0.392868190519302]
Robot Arm 1 togoal: 22.2712203396228
Mobile Platform 2 Pose: [4.59867917968814;-5.1828663707523;-3.12936536624705]
Robot Arm 2 end: [-5.29360767462504;-3.84149210773479;0.888793549134797]
Robot Arm 1 togoal: 7.77669808954654
Step 164:
Mobile Platform 1 Pose: [-1.33038880810416;-3.11651111177353;-7.3785572390335]
Robot Arm 1 end: [-3.82202385724433;-7.5160414510145;0.317615234606001]
Robot Arm 1 togoal: 22.3452050005694
Mobile Platform 2 Pose: [4.49485998617774;-5.29830670676305;-3.13673381580427]
Robot Arm 2 end: [-5.60279479818386;-3.84175738125683;0.721477603074959]
Robot Arm 1 togoal: 7.60911307586951
Step 165:
Mobile Platform 1 Pose: [-0.950921184153906;-3.32794138803308;-7.44917847391395]
Robot Arm 1 end: [-4.0269247216038;-7.61044437007352;0.294155392200807]
Robot Arm 1 togoal: 22.5474503159155
Mobile Platform 2 Pose: [4.7425511324277;-5.54246502900716;-3.1904619681781]
Robot Arm 2 end: [-5.65668365880674;-3.89202014225209;0.916331881063864]
Robot Arm 1 togoal: 7.51913813654121
Step 166:
Mobile Platform 1 Pose: [-0.713650490237377;-3.47402305325774;-7.49029385608496]
Robot Arm 1 end: [-4.12500825944465;-7.63196882091468;0.229320776368194]
Robot Arm 1 togoal: 22.6289773973074
Mobile Platform 2 Pose: [4.6953110168266;-5.68805882049145;-3.20604076057933]
Robot Arm 2 end: [-5.96853023526748;-3.89777236080794;0.860329704119445]
Robot Arm 1 togoal: 7.34346075527954
Step 167:
Mobile Platform 1 Pose: [-0.69190873696728;-3.3592878779597;-7.55315200418302]
Robot Arm 1 end: [-4.03668435490539;-7.69749368054834;0.303724276448885]
Robot Arm 1 togoal: 22.621042204979
Mobile Platform 2 Pose: [4.89752592847912;-5.93226420128368;-3.2595546094519]
Robot Arm 2 end: [-6.03278912330244;-3.98632963845815;0.848590466796035]
Robot Arm 1 togoal: 7.23568434481618
Step 168:
Mobile Platform 1 Pose: [-0.721579241886868;-3.2378558631982;-7.59597805023825]
Robot Arm 1 end: [-3.87572337397416;-7.72102122798087;0.237705615725394]
Robot Arm 1 togoal: 22.5436717725909
Mobile Platform 2 Pose: [4.85009438248694;-6.08203230053162;-3.30473321972802]
Robot Arm 2 end: [-6.36080512789973;-4.00666975660063;0.850204293634773]
Robot Arm 1 togoal: 7.0437002777833
Step 169:
Mobile Platform 1 Pose: [-0.844539843964006;-3.1254158049238;-7.59866243147114]
Robot Arm 1 end: [-3.80059919810764;-7.73411534216121;0.315506125426156]
Robot Arm 1 togoal: 22.5035241835034
Mobile Platform 2 Pose: [5.06163086954554;-6.32269915453663;-3.3724049670974]
Robot Arm 2 end: [-6.42311266613963;-4.11485032693301;0.800238831464805]
Robot Arm 1 togoal: 6.92449321868391
Step 170:
Mobile Platform 1 Pose: [-0.993351486329196;-2.96354484792975;-7.61406843579089]
Robot Arm 1 end: [-3.68385270651445;-7.74568671168258;0.416704248406578]
Robot Arm 1 togoal: 22.4360578281871
Mobile Platform 2 Pose: [4.98247183471632;-6.43809616629287;-3.43289623183361]
Robot Arm 2 end: [-6.7193558633863;-4.14920708963044;0.807411465222353]
Robot Arm 1 togoal: 6.74563653246264
Step 171:
Mobile Platform 1 Pose: [-0.634895468480383;-3.10130597789241;-7.71753338899473]
Robot Arm 1 end: [-3.8534741136001;-7.87268102466813;0.487263342788456]
Robot Arm 1 togoal: 22.6366977163672
Mobile Platform 2 Pose: [5.14493107421445;-6.67215152160431;-3.52074889627839]
Robot Arm 2 end: [-6.82022004444589;-4.28318156004312;0.724012706107918]
Robot Arm 1 togoal: 6.59000776841728
Step 172:
Mobile Platform 1 Pose: [-0.477412774882214;-3.23013143956476;-7.73514947428498]
Robot Arm 1 end: [-3.9635139839799;-7.87984744029567;0.401131891521199]
Robot Arm 1 togoal: 22.7139334297131
Mobile Platform 2 Pose: [5.10805769150373;-6.7933864228058;-3.62140732240946]
Robot Arm 2 end: [-7.113933110895;-4.33798749159293;0.808772318335731]
Robot Arm 1 togoal: 6.39492267383079
Step 173:
Mobile Platform 1 Pose: [-0.244629264208608;-3.39695924724051;-7.76395085765602]
Robot Arm 1 end: [-4.11666605881492;-7.92293130910626;0.345713038464011]
Robot Arm 1 togoal: 22.8449781520796
Mobile Platform 2 Pose: [5.26425108702225;-7.01792151016225;-3.72539957580861]
Robot Arm 2 end: [-7.30772253219741;-4.47091098827399;0.725156591882505]
Robot Arm 1 togoal: 6.20101201365316
Step 174:
Mobile Platform 1 Pose: [-0.0369430762470237;-3.54601524393948;-7.79242983340939]
Robot Arm 1 end: [-4.23023139315338;-7.93728756830348;0.272399269838079]
Robot Arm 1 togoal: 22.9304333712659
Mobile Platform 2 Pose: [5.46110022612181;-7.23801361528264;-3.84397219182165]
Robot Arm 2 end: [-7.41124233017747;-4.60961309088185;0.826519263710793]
Robot Arm 1 togoal: 6.01998677707303
Step 175:
Mobile Platform 1 Pose: [0.321297189227114;-3.71696270969668;-7.86999502006516]
Robot Arm 1 end: [-4.44774966078565;-8.04098981082964;0.342173360721359]
Robot Arm 1 togoal: 23.1431289125015
Mobile Platform 2 Pose: [5.34146378385284;-7.22689706462322;-3.87689632651085]
Robot Arm 2 end: [-7.52184292961353;-4.6274457697291;0.842136532776441]
Robot Arm 1 togoal: 5.95537211514436
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 90)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',90,0)">第 90 行</a>)
    real_time_2D_plot(human_poses{k-1}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000999e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.770711281294394;-9.98997741396601;-9.82820014583124]
Robot Arm 1 end: [-9.84483150778502;-9.82585448595577;1.50725011239015]
Robot Arm 1 togoal: 28.0514176598987
Mobile Platform 2 Pose: [-2.35260699045331;9.99567012367058;9.81851212164378]
Robot Arm 2 end: [9.77362939383858;9.83020782788875;1.48915678824555]
Robot Arm 1 togoal: 28.0041885419355

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994620e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.530531694765162;-9.78450413570073;-9.75690415805299]
Robot Arm 1 end: [-9.66144965565694;-9.73136130979408;1.5106292582045]
Robot Arm 1 togoal: 27.8549695611368
Mobile Platform 2 Pose: [-2.15547032577163;9.86197581573249;9.8148916772762]
Robot Arm 2 end: [9.54850948253733;9.88346900111851;1.46006104850215]
Robot Arm 1 togoal: 27.8836919374588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.280033247152714;-9.5870720683219;-9.68786049711598]
Robot Arm 1 end: [-9.42760919715894;-9.62018484327968;1.50138091722694]
Robot Arm 1 togoal: 27.6113027094052
Mobile Platform 2 Pose: [-2.00112519317392;9.74652358237442;9.82809196755915]
Robot Arm 2 end: [9.24985727275685;9.99325763611483;1.34370146553084]
Robot Arm 1 togoal: 27.7546174662145

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.878127e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.213052795876252;-9.43174979122518;-9.56796614298057]
Robot Arm 1 end: [-9.26696976134994;-9.47490046386192;1.49866273776324]
Robot Arm 1 togoal: 27.3950428146296
Mobile Platform 2 Pose: [-1.89649020555169;9.65419607461748;9.8523035661954]
Robot Arm 2 end: [9.00458451727503;9.99213666723393;1.20765646026526]
Robot Arm 1 togoal: 27.5854661981541

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.926789e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0751282428815386;-9.27136279584706;-9.50352125955366]
Robot Arm 1 end: [-9.05062434932384;-9.32120588282888;1.46309762644707]
Robot Arm 1 togoal: 27.1337177244582
Mobile Platform 2 Pose: [-1.75761023641085;9.52841187378403;9.87454064693542]
Robot Arm 2 end: [8.81501083526358;10.0361716111206;1.1286913190314]
Robot Arm 1 togoal: 27.4883013600791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.820771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.00250980054953168;-9.21614520074308;-9.50390327241158]
Robot Arm 1 end: [-9.15303514172708;-9.42014827696597;1.49106907580758]
Robot Arm 1 togoal: 27.2760299548853
Mobile Platform 2 Pose: [-1.59169692061011;9.44670370294246;9.86664912889522]
Robot Arm 2 end: [8.679904690655;9.99986102732668;1.04271371881697]
Robot Arm 1 togoal: 27.3708245789399

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880782e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.0388883974993157;-9.1619134559037;-9.51043524382403]
Robot Arm 1 end: [-9.17599612556879;-9.46596979705345;1.48011724156525]
Robot Arm 1 togoal: 27.3247960458548
Mobile Platform 2 Pose: [-1.5065346406365;9.39900745085128;9.86197040262286]
Robot Arm 2 end: [8.63536968333956;9.83538998611164;1.06346055719492]
Robot Arm 1 togoal: 27.2200117832008

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-0.0606503300912776;-9.08239395641017;-9.52011874433356]
Robot Arm 1 end: [-9.13174876412589;-9.46692519951133;1.46597018648706]
Robot Arm 1 togoal: 27.294468593057
Mobile Platform 2 Pose: [-1.44291214823887;9.36113383159866;9.8546794064431]
Robot Arm 2 end: [8.65261193242396;9.69974922501956;1.11103400280827]
Robot Arm 1 togoal: 27.132418313691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954417e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-0.108465236443875;-8.93981472601767;-9.48047470467915]
Robot Arm 1 end: [-8.98752016297892;-9.35811701844276;1.46235598995542]
Robot Arm 1 togoal: 27.1157897830612
Mobile Platform 2 Pose: [-1.38074970696818;9.3253250125419;9.84421744681893]
Robot Arm 2 end: [8.66759007071185;9.60278451263746;1.11876228410613]
Robot Arm 1 togoal: 27.0723082665133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992121e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-0.227353182821805;-8.76376981354699;-9.35873958128988]
Robot Arm 1 end: [-8.8611290457707;-9.08713799170846;1.42543596735558]
Robot Arm 1 togoal: 26.8341230167872
Mobile Platform 2 Pose: [-1.31938570628856;9.29029692234151;9.82963356495981]
Robot Arm 2 end: [8.63888610034572;9.51350829376706;1.0557675901271]
Robot Arm 1 togoal: 26.9889148019659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-0.352617950660105;-8.6319893244171;-9.2264052651218]
Robot Arm 1 end: [-8.78683220286217;-8.80397048682128;1.36995846781328]
Robot Arm 1 togoal: 26.5811433883531
Mobile Platform 2 Pose: [-1.26573137085896;9.26078482628357;9.81574261938248]
Robot Arm 2 end: [8.63031764797234;9.37706742534872;0.981335475569302]
Robot Arm 1 togoal: 26.8858832004788

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-0.490942707961035;-8.52202644215624;-9.0814874558887]
Robot Arm 1 end: [-8.76212753139997;-8.55277993319905;1.2765592924479]
Robot Arm 1 togoal: 26.3871715804051
Mobile Platform 2 Pose: [-1.19070162136616;9.22341116289018;9.80287981487735]
Robot Arm 2 end: [8.67201017533547;9.22462899563349;0.904899867961768]
Robot Arm 1 togoal: 26.8069002941459

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-0.639201048080114;-8.43607585708193;-8.93908835503356]
Robot Arm 1 end: [-8.83758814818822;-8.40232569659463;1.16422369446958]
Robot Arm 1 togoal: 26.3368108854602
Mobile Platform 2 Pose: [-1.06247444781621;9.15993871253121;9.78191218013082]
Robot Arm 2 end: [8.73790975328102;9.08142461102036;0.822408557907529]
Robot Arm 1 togoal: 26.7525336577888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-0.628645383894545;-8.46238413561934;-8.84767829262856]
Robot Arm 1 end: [-8.66347579953201;-8.29131147497066;1.26956330693836]
Robot Arm 1 togoal: 26.1335153548987
Mobile Platform 2 Pose: [-0.848082164597224;9.04757804563915;9.7313991397021]
Robot Arm 2 end: [8.80189064450609;8.95045095506328;0.735519463798255]
Robot Arm 1 togoal: 26.7066969668841

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993807e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-0.680900599085814;-8.43563546017657;-8.69928158488519]
Robot Arm 1 end: [-8.60064393231615;-8.11325528904314;1.25626636011042]
Robot Arm 1 togoal: 25.9642842335957
Mobile Platform 2 Pose: [-0.66207418945679;8.90538636425985;9.57575704675167]
Robot Arm 2 end: [8.75546024327431;8.76877470256299;0.663739454881766]
Robot Arm 1 togoal: 26.5474874218791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998344e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-0.617408073966987;-8.47023140471197;-8.56458901760298]
Robot Arm 1 end: [-8.45332855964348;-8.05807263888524;1.35283379801139]
Robot Arm 1 togoal: 25.8195197909043
Mobile Platform 2 Pose: [-0.897886538941678;8.96449332930697;9.43696476241411]
Robot Arm 2 end: [8.69845258486172;8.64106481176717;0.739044019776545]
Robot Arm 1 togoal: 26.4146338520872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-0.550924345892755;-8.47883939139649;-8.40047589152532]
Robot Arm 1 end: [-8.43361962380529;-8.02012026329913;1.43317730671636]
Robot Arm 1 togoal: 25.7784945170264
Mobile Platform 2 Pose: [-0.906099755811399;8.93252005865099;9.26584400378437]
Robot Arm 2 end: [8.77268998647061;8.43714786714181;0.798632403799485]
Robot Arm 1 togoal: 26.3223123176332

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-0.478097754005546;-8.52277459608488;-8.36481605626173]
Robot Arm 1 end: [-8.40238950219456;-8.13054172290625;1.47706463257926]
Robot Arm 1 togoal: 25.8334359478765
Mobile Platform 2 Pose: [-1.07558750599369;9.00840995475137;9.22242384279819]
Robot Arm 2 end: [8.76115723899512;8.44284810589671;0.921502694468084]
Robot Arm 1 togoal: 26.3149969451417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-0.435683699695462;-8.54902018259617;-8.33954463951443]
Robot Arm 1 end: [-8.3981584705746;-8.26718456530172;1.48804167355434]
Robot Arm 1 togoal: 25.9264990560325
Mobile Platform 2 Pose: [-1.27422638170888;9.09577443756223;9.14552281257661]
Robot Arm 2 end: [8.81890749366157;8.45667622227799;1.0720565551296]
Robot Arm 1 togoal: 26.3628854006098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-0.309140568130136;-8.62389618627006;-8.28626110538303]
Robot Arm 1 end: [-8.50729234388045;-8.38534633454475;1.46985021576179]
Robot Arm 1 togoal: 26.08722764196
Mobile Platform 2 Pose: [-1.43952502413914;9.15218951875675;8.96785165692817]
Robot Arm 2 end: [8.90185229964591;8.33563464721566;1.13516423642514]
Robot Arm 1 togoal: 26.3367497522048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.323028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-0.347570243410041;-8.63183964589212;-8.15180231856537]
Robot Arm 1 end: [-8.46078413021357;-8.28666598371203;1.43133369454934]
Robot Arm 1 togoal: 25.9848175585
Mobile Platform 2 Pose: [-1.39923064957826;9.13477543932798;8.80371862732173]
Robot Arm 2 end: [8.7850815052059;8.17284725848866;1.04451846395915]
Robot Arm 1 togoal: 26.1411304134266

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.755989e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-0.366965995355082;-8.64769083487106;-8.01324314472416]
Robot Arm 1 end: [-8.43490110591236;-8.24910796170017;1.36451282564281]
Robot Arm 1 togoal: 25.9403162744461
Mobile Platform 2 Pose: [-1.4774057442873;9.14476417053453;8.61230752872338]
Robot Arm 2 end: [8.90234869729349;7.98746819789215;1.11526494997683]
Robot Arm 1 togoal: 26.0962146560583
Step 25:
Mobile Platform 1 Pose: [-0.491967955865556;-8.55498582497503;-7.91672545368362]
Robot Arm 1 end: [-8.34260232935448;-8.23531081054624;1.36753935264013]
Robot Arm 1 togoal: 25.8650569444638
Mobile Platform 2 Pose: [-1.39270658465331;9.10027024858723;8.4463071120877]
Robot Arm 2 end: [8.75529064134336;7.82616861387436;1.03242811329978]
Robot Arm 1 togoal: 25.8799520281629
Step 26:
Mobile Platform 1 Pose: [-0.611703605878295;-8.4523490026128;-7.90810969569635]
Robot Arm 1 end: [-8.24634532497743;-8.04931326556307;1.45168518840245]
Robot Arm 1 togoal: 25.665378072802
Mobile Platform 2 Pose: [-1.58836841437344;9.16126516549548;8.29030057325798]
Robot Arm 2 end: [8.92173394267916;7.68428558481285;1.09719417946321]
Robot Arm 1 togoal: 25.9026173082234
Step 27:
Mobile Platform 1 Pose: [-0.805157318849628;-8.30493418886807;-7.88151934165363]
Robot Arm 1 end: [-8.17642213693852;-7.85458872582617;1.4986056322419]
Robot Arm 1 togoal: 25.4787982796735
Mobile Platform 2 Pose: [-1.37474832575903;9.03780163598655;8.14040509606234]
Robot Arm 2 end: [8.70474262283727;7.52464311699027;1.02630206526793]
Robot Arm 1 togoal: 25.6364045648193
Step 28:
Mobile Platform 1 Pose: [-1.00229545187201;-8.13409492732632;-7.78402855042757]
Robot Arm 1 end: [-8.09891710742607;-7.61931820096122;1.50266129601015]
Robot Arm 1 togoal: 25.2588897348293
Mobile Platform 2 Pose: [-1.61764241667244;9.12195335289267;8.03248193989886]
Robot Arm 2 end: [8.89163007330717;7.43157323377574;1.10274235161926]
Robot Arm 1 togoal: 25.708527217474
Step 29:
Mobile Platform 1 Pose: [-1.10803337715637;-8.02868644551668;-7.63379007856017]
Robot Arm 1 end: [-8.0433223957475;-7.42682365198413;1.49400647257514]
Robot Arm 1 togoal: 25.0849834427842
Mobile Platform 2 Pose: [-1.40365595116085;8.98464542668472;7.86599329072659]
Robot Arm 2 end: [8.66284621543055;7.25756456368136;1.03168674128257]
Robot Arm 1 togoal: 25.4237052963489

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.736931e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-1.22613018934619;-7.94890355981858;-7.4930948616529]
Robot Arm 1 end: [-8.01883523749166;-7.26928517967096;1.48678257769629]
Robot Arm 1 togoal: 24.9581210183111
Mobile Platform 2 Pose: [-1.64356060256891;9.06559290605252;7.77146876000809]
Robot Arm 2 end: [8.94368065738967;7.11279903533916;1.06867469902803]
Robot Arm 1 togoal: 25.5326364845263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.616936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-1.32780927777427;-7.90876027242118;-7.37977124346862]
Robot Arm 1 end: [-7.97596301192846;-7.13641210313145;1.48341099158779]
Robot Arm 1 togoal: 24.8353232092543
Mobile Platform 2 Pose: [-1.5146033649243;8.97692939617356;7.59082530750575]
Robot Arm 2 end: [8.76008466069691;6.92353487392469;0.991435151855236]
Robot Arm 1 togoal: 25.2710753240508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993538e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.41320748062622;-7.88716597955583;-7.28347669183898]
Robot Arm 1 end: [-7.96370184736632;-7.02999393792135;1.47817367676285]
Robot Arm 1 togoal: 24.7531232712293
Mobile Platform 2 Pose: [-1.73627162805894;9.04543000631225;7.46650093667693]
Robot Arm 2 end: [8.91477184910729;6.7275162252775;0.91301947843019]
Robot Arm 1 togoal: 25.257633172673

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959372e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.490804485129;-7.86631449080507;-7.24343604346549]
Robot Arm 1 end: [-7.94043868116454;-6.98213022892621;1.47061176470077]
Robot Arm 1 togoal: 24.7033322885616
Mobile Platform 2 Pose: [-1.53578313969102;8.91097773530775;7.30234298529301]
Robot Arm 2 end: [8.67565423545589;6.57484591374137;0.830827356710415]
Robot Arm 1 togoal: 24.9796300305692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.5936131815957;-7.81746153770647;-7.23186394113836]
Robot Arm 1 end: [-7.85710447747499;-6.9639117073294;1.46578761957019]
Robot Arm 1 togoal: 24.630337576665
Mobile Platform 2 Pose: [-1.72731322787985;8.95511882399017;7.16080829230364]
Robot Arm 2 end: [8.82628330237061;6.3691646782611;0.715005019965931]
Robot Arm 1 togoal: 24.960531307506

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.014536e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.70972371395942;-7.75558435309665;-7.22551679048785]
Robot Arm 1 end: [-7.75239022150043;-6.95852943147428;1.46199198764974]
Robot Arm 1 togoal: 24.5508158927585
Mobile Platform 2 Pose: [-1.51999019037219;8.82011291927974;7.00741051121637]
Robot Arm 2 end: [8.57130937275556;6.25967488424458;0.629075557216485]
Robot Arm 1 togoal: 24.6995159541957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.800947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.83451332358808;-7.68649556017655;-7.21994591361693]
Robot Arm 1 end: [-7.65398943328238;-6.96287969195063;1.45874088445349]
Robot Arm 1 togoal: 24.4827797962467
Mobile Platform 2 Pose: [-1.61691793104931;8.81379446287667;6.84131367027345]
Robot Arm 2 end: [8.49269377189909;6.11912923799737;0.507410504676493]
Robot Arm 1 togoal: 24.5526661280914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000618e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-1.9735707440706;-7.60763121383692;-7.21343942887122]
Robot Arm 1 end: [-7.57060049048381;-6.98060351431802;1.4593866798575]
Robot Arm 1 togoal: 24.4350300492526
Mobile Platform 2 Pose: [-1.56771821091351;8.71306491550288;6.65925226019932]
Robot Arm 2 end: [8.2955096158813;6.02171011604392;0.425965341730121]
Robot Arm 1 togoal: 24.34378705235

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.885067e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.13432488739624;-7.51417950648373;-7.20470033607455]
Robot Arm 1 end: [-7.50873258785362;-7.03244918474058;1.46996879890584]
Robot Arm 1 togoal: 24.4266789852248
Mobile Platform 2 Pose: [-1.50402780114831;8.57184851845037;6.48156784562853]
Robot Arm 2 end: [8.01807383061346;5.93769295841505;0.42574647064734]
Robot Arm 1 togoal: 24.08029529918

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.904591e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.3317438219411;-7.39798065220005;-7.18949380979464]
Robot Arm 1 end: [-7.4810827247748;-7.1456865867224;1.48286330413212]
Robot Arm 1 togoal: 24.4860016370191
Mobile Platform 2 Pose: [-1.66479449205893;8.52275150766905;6.33690011899042]
Robot Arm 2 end: [8.10940159849721;5.7184295869662;0.345339666670701]
Robot Arm 1 togoal: 24.0083658784898

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.018340e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.56387661332793;-7.26266285920729;-7.16476072416941]
Robot Arm 1 end: [-7.42814262032825;-7.28170311169579;1.47404853739517]
Robot Arm 1 togoal: 24.5438306316504
Mobile Platform 2 Pose: [-1.46270261024642;8.32614161672252;6.1824815583151]
Robot Arm 2 end: [7.75922843770563;5.64475151946988;0.426300130976346]
Robot Arm 1 togoal: 23.6927586534749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957779e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.7913192973342;-7.1037345197218;-7.1226583471552]
Robot Arm 1 end: [-7.31796003033719;-7.4258195436791;1.4197164337449]
Robot Arm 1 togoal: 24.5678893017292
Mobile Platform 2 Pose: [-1.70858848997431;8.34002229523287;6.06428314271953]
Robot Arm 2 end: [7.92383138653025;5.44739792149574;0.347080023558834]
Robot Arm 1 togoal: 23.6910093121607

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.98539949172545;-6.87015956081314;-7.03353630402788]
Robot Arm 1 end: [-6.97205639642644;-7.27744487426763;1.44172258409611]
Robot Arm 1 togoal: 24.2191470662265
Mobile Platform 2 Pose: [-1.58414581908488;8.16276174290591;5.88564632753486]
Robot Arm 2 end: [7.60102178461831;5.35329920742868;0.378281349704432]
Robot Arm 1 togoal: 23.384301792638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-3.10055170747329;-6.66444908261591;-7.02468123926042]
Robot Arm 1 end: [-6.73898890883478;-7.18336998458819;1.43545784118026]
Robot Arm 1 togoal: 23.9889406014162
Mobile Platform 2 Pose: [-1.82911329258329;8.16622724096902;5.76026886864605]
Robot Arm 2 end: [7.74944843204278;5.14956348436829;0.32801696068279]
Robot Arm 1 togoal: 23.3661165352899

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.600231e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.22106916296952;-6.51645094870625;-7.06423734917106]
Robot Arm 1 end: [-6.58138514708413;-7.15042575033107;1.41922577083416]
Robot Arm 1 togoal: 23.8556030065744
Mobile Platform 2 Pose: [-1.66464794046475;7.96431272126698;5.58255159075045]
Robot Arm 2 end: [7.41105866553815;5.09482323665232;0.302968741523319]
Robot Arm 1 togoal: 23.0755769710579

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.892103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.29447643505363;-6.45145602385434;-7.0903250349142]
Robot Arm 1 end: [-6.49087917364324;-7.11783151785943;1.40790368358611]
Robot Arm 1 togoal: 23.7693509076354
Mobile Platform 2 Pose: [-1.85550920852954;7.94323295642117;5.47010077097711]
Robot Arm 2 end: [7.53864830227307;4.91897166740651;0.228707344388011]
Robot Arm 1 togoal: 23.0618715674043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.845393e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.36013216319206;-6.39613081460968;-7.09994779548381]
Robot Arm 1 end: [-6.3772773269029;-7.08037696489647;1.38821061207889]
Robot Arm 1 togoal: 23.6637301143923
Mobile Platform 2 Pose: [-1.71072281775641;7.73656861729483;5.28990050899297]
Robot Arm 2 end: [7.28197676791879;4.83641104240027;0.151592087860747]
Robot Arm 1 togoal: 22.8179992912563

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.523481e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.43366105618918;-6.33904203039483;-7.10357645481889]
Robot Arm 1 end: [-6.27212997691042;-7.03172197990912;1.35180392824364]
Robot Arm 1 togoal: 23.5561116251618
Mobile Platform 2 Pose: [-1.78155377578551;7.60784369400395;5.12748391521719]
Robot Arm 2 end: [7.1962336548459;4.55045340045736;0.218472443869514]
Robot Arm 1 togoal: 22.5637480066015

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.572232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.50675979987343;-6.29045333100322;-7.1048214214234]
Robot Arm 1 end: [-6.19141129632944;-6.98204202863464;1.3015283823068]
Robot Arm 1 togoal: 23.4648613680156
Mobile Platform 2 Pose: [-1.55651873972125;7.37151318657328;4.96175214676045]
Robot Arm 2 end: [6.88128253749596;4.45877861683008;0.197861214259977]
Robot Arm 1 togoal: 22.2662227476417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.471523e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.284124e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.57307231671237;-6.24962959883336;-7.10439671995214]
Robot Arm 1 end: [-6.13860549830164;-6.93244375567673;1.24046675579298]
Robot Arm 1 togoal: 23.3931814396861
Mobile Platform 2 Pose: [-1.74156917519049;7.32850676523451;4.82034689124971]
Robot Arm 2 end: [6.89052164793999;4.19614348359497;0.277822023716899]
Robot Arm 1 togoal: 22.0989977787922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.665619e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.828878e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-3.62407591535976;-6.21793451986153;-7.10337766408233]
Robot Arm 1 end: [-6.1031798160211;-6.88095946546341;1.17439294776933]
Robot Arm 1 togoal: 23.3323665561465
Mobile Platform 2 Pose: [-1.46712172980307;7.09041981103442;4.68177070586141]
Robot Arm 2 end: [6.52983339558321;4.17814142893102;0.264644199801606]
Robot Arm 1 togoal: 21.8136085130636

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.951236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-3.66859251113105;-6.18978071159296;-7.10219158574749]
Robot Arm 1 end: [-6.078519076812;-6.8286804744448;1.10278274238403]
Robot Arm 1 togoal: 23.2787063190577
Mobile Platform 2 Pose: [-1.44518015925981;6.9282994918632;4.5320253287547]
Robot Arm 2 end: [6.42549468142277;3.90022822568157;0.354363951807352]
Robot Arm 1 togoal: 21.5493447678677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000057e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-3.70845305849548;-6.1635551428844;-7.10108007728866]
Robot Arm 1 end: [-6.0585941148823;-6.77642908869426;1.02303626079046]
Robot Arm 1 togoal: 23.2287534275967
Mobile Platform 2 Pose: [-1.31580091142329;6.75351664271358;4.37908764244834]
Robot Arm 2 end: [6.15978126498811;3.83459303326162;0.378538681364067]
Robot Arm 1 togoal: 21.3035140949284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000264e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-3.7453590066297;-6.13815967342033;-7.1000057208367]
Robot Arm 1 end: [-6.04158814073836;-6.72131088936092;0.930969952633987]
Robot Arm 1 togoal: 23.1793641714464
Mobile Platform 2 Pose: [-1.32393676581562;6.62649643227401;4.2391987928695]
Robot Arm 2 end: [6.10947164028969;3.57532816787148;0.472801470012528]
Robot Arm 1 togoal: 21.0927317233442

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-3.78013039612414;-6.11348712071264;-7.09883178362965]
Robot Arm 1 end: [-6.02896812919038;-6.65926255174296;0.819169808523996]
Robot Arm 1 togoal: 23.1290167035301
Mobile Platform 2 Pose: [-1.20340415803821;6.467883793944;4.09403512364629]
Robot Arm 2 end: [5.90983588739202;3.47027877416752;0.512996627167101]
Robot Arm 1 togoal: 20.8707297193265

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977993e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-3.80723954023518;-6.09291301324315;-7.09790591797435]
Robot Arm 1 end: [-5.99721818338345;-6.60139637060918;0.729709785780526]
Robot Arm 1 togoal: 23.0682355472622
Mobile Platform 2 Pose: [-1.1383927153657;6.32895987141101;3.95121860080304]
Robot Arm 2 end: [5.88252625179115;3.22725415630899;0.606521978243091]
Robot Arm 1 togoal: 20.6893973500125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.812091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-3.82882703904885;-6.0752970707509;-7.0984294011453]
Robot Arm 1 end: [-5.97779774200119;-6.56355894138005;0.646572074145474]
Robot Arm 1 togoal: 23.0305477504636
Mobile Platform 2 Pose: [-0.967507887137015;6.18106179554736;3.81869981641198]
Robot Arm 2 end: [5.67824899126807;3.15541173903533;0.70652360545015]
Robot Arm 1 togoal: 20.4825711582561

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949183e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-3.85466985202238;-6.05590981242092;-7.10073500212772]
Robot Arm 1 end: [-5.94853185912838;-6.53421848517365;0.57251730984782]
Robot Arm 1 togoal: 22.9920783921465
Mobile Platform 2 Pose: [-0.979734682181815;6.10529457290346;3.67942256793099]
Robot Arm 2 end: [5.72458981128706;2.94085707100451;0.791592463882527]
Robot Arm 1 togoal: 20.3779425315498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986083e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-3.89486919333202;-6.02935971359595;-7.10425061866907]
Robot Arm 1 end: [-5.91353270303717;-6.51039499912841;0.511464927559376]
Robot Arm 1 togoal: 22.9532748683138
Mobile Platform 2 Pose: [-0.872383007399513;5.99337251767564;3.5339580150843]
Robot Arm 2 end: [5.65211426399048;2.81269557442244;0.865897361753789]
Robot Arm 1 togoal: 20.2381572353432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984299e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-3.94492624885346;-5.99855364414466;-7.10838646641971]
Robot Arm 1 end: [-5.88426945118795;-6.49018616134189;0.462474599948687]
Robot Arm 1 togoal: 22.9206616757939
Mobile Platform 2 Pose: [-0.887061244458391;5.93838509374609;3.38337893323307]
Robot Arm 2 end: [5.72812058190189;2.63455175243321;0.953333557142252]
Robot Arm 1 togoal: 20.1823665662406

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982209e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984089e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-3.99690248933349;-5.96813542343933;-7.11341215133933]
Robot Arm 1 end: [-5.85826013314568;-6.47358128362411;0.424535717999153]
Robot Arm 1 togoal: 22.8924775566209
Mobile Platform 2 Pose: [-0.838952401784367;5.87184814300329;3.22292996501152]
Robot Arm 2 end: [5.75966755182851;2.5100804565853;1.04105987165854]
Robot Arm 1 togoal: 20.1270856534469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002668e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-4.06529909636159;-5.93155553653526;-7.11957521274945]
Robot Arm 1 end: [-5.79821564749105;-6.46990366697104;0.388688406678764]
Robot Arm 1 togoal: 22.8500213126445
Mobile Platform 2 Pose: [-0.913020155530733;5.86695590286618;3.06340976177083]
Robot Arm 2 end: [5.85980358143161;2.37702026924174;1.12879901710117]
Robot Arm 1 togoal: 20.1215760316581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.743045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991227e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-4.09672473074866;-5.91591722321928;-7.12403747640995]
Robot Arm 1 end: [-5.74972360452566;-6.47675608971946;0.373893856956465]
Robot Arm 1 togoal: 22.822207109797
Mobile Platform 2 Pose: [-0.789299253154896;5.78116171149643;2.88417347750494]
Robot Arm 2 end: [5.75128500943115;2.35336990941166;1.27187492665424]
Robot Arm 1 togoal: 20.0192604975623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.826990e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009813e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-4.13733294449514;-5.88915262154753;-7.12344796181667]
Robot Arm 1 end: [-5.75048690839741;-6.48386125322609;0.379778444339667]
Robot Arm 1 togoal: 22.827568996858
Mobile Platform 2 Pose: [-1.01041420800845;5.83398212714067;2.71945663548866]
Robot Arm 2 end: [5.83466031386769;2.25924624727799;1.34571812730368]
Robot Arm 1 togoal: 20.0263903273124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.400448e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005656e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-4.18041276431633;-5.8626331001464;-7.12400200521948]
Robot Arm 1 end: [-5.63423491367345;-6.55442124384555;0.345911850533432]
Robot Arm 1 togoal: 22.8004165798265
Mobile Platform 2 Pose: [-0.600561129480797;5.58913680583522;2.52799543541753]
Robot Arm 2 end: [5.53755972181552;2.34764500239347;1.44419182628515]
Robot Arm 1 togoal: 19.8465613825082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.786897e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.919498e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-4.21172389032999;-5.84289157715506;-7.12835682622549]
Robot Arm 1 end: [-5.64084587764919;-6.56253841029244;0.347648896429459]
Robot Arm 1 togoal: 22.8107538492931
Mobile Platform 2 Pose: [-0.205985563676439;5.38672059627461;2.3593134310714]
Robot Arm 2 end: [5.16021591738417;2.01461111303539;1.21547022746994]
Robot Arm 1 togoal: 19.3462237995192

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.348282e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967088e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-4.23467306661421;-5.82810226645656;-7.13220377826655]
Robot Arm 1 end: [-5.6705741062815;-6.5695124533123;0.332915801219284]
Robot Arm 1 togoal: 22.8369695849586
Mobile Platform 2 Pose: [-0.235266196097706;5.39239995670732;2.34676208987189]
Robot Arm 2 end: [5.208366630713;1.93860127686475;1.23890721559832]
Robot Arm 1 togoal: 19.3366023996242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.866907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000328e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-4.253825507607;-5.81579165993254;-7.13531380011674]
Robot Arm 1 end: [-5.71499667312657;-6.57967432255629;0.314167889034587]
Robot Arm 1 togoal: 22.8758205278514
Mobile Platform 2 Pose: [-0.315715373687527;5.41238275643489;2.32325906666636]
Robot Arm 2 end: [5.30063339304722;1.85734567910779;1.2427383012731]
Robot Arm 1 togoal: 19.3593248424439

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010246e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-4.27082375372806;-5.8050284475795;-7.13778912320014]
Robot Arm 1 end: [-5.76093436896876;-6.5930280025302;0.291928134648776]
Robot Arm 1 togoal: 22.9182514194668
Mobile Platform 2 Pose: [-0.500403302389168;5.46765068127878;2.28726445064572]
Robot Arm 2 end: [5.48077783430787;1.73792581713986;1.19902559992762]
Robot Arm 1 togoal: 19.4303127190012

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948366e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.014238e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-4.28530410998474;-5.79581254095102;-7.13988835529199]
Robot Arm 1 end: [-5.81515541864742;-6.60993086418859;0.268601783516461]
Robot Arm 1 togoal: 22.9690540841748
Mobile Platform 2 Pose: [-0.73110118569891;5.5457845767815;2.25285157530561]
Robot Arm 2 end: [5.61869862077975;1.63418888981192;1.16084476898996]
Robot Arm 1 togoal: 19.478906784545

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011857e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-4.29865696624061;-5.78727928473361;-7.14172041801919]
Robot Arm 1 end: [-5.88736749151609;-6.64126611753904;0.24532641475855]
Robot Arm 1 togoal: 23.0427326266074
Mobile Platform 2 Pose: [-0.947128419847152;5.62836985621233;2.22190003894837]
Robot Arm 2 end: [5.60204507447168;1.49507897635088;1.03959194378051]
Robot Arm 1 togoal: 19.3853657405524

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991992e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024709e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-4.30986865033163;-5.78011208006536;-7.143110753445]
Robot Arm 1 end: [-5.97362045997039;-6.68107859825923;0.224157017510434]
Robot Arm 1 togoal: 23.1321580385774
Mobile Platform 2 Pose: [-1.20667320908345;5.71342596949713;2.14454210338915]
Robot Arm 2 end: [5.55177545797898;1.39714615750685;0.992315786632094]
Robot Arm 1 togoal: 19.2881354056953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.950114e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.938139e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-4.32143174497443;-5.77277259429072;-7.14424195548953]
Robot Arm 1 end: [-6.08660049082681;-6.71273485959814;0.247308226003568]
Robot Arm 1 togoal: 23.2317995982628
Mobile Platform 2 Pose: [-1.11402461183155;5.6096881410637;1.95818463687936]
Robot Arm 2 end: [5.57130977010107;1.15795901748359;0.938557994984577]
Robot Arm 1 togoal: 19.1652022186434

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970116e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002784e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-4.32889138624467;-5.76817926305596;-7.14489624263064]
Robot Arm 1 end: [-6.18024001945219;-6.77008483937784;0.264465192130746]
Robot Arm 1 togoal: 23.336983359689
Mobile Platform 2 Pose: [-1.11276756300802;5.50784940580751;1.75155771217474]
Robot Arm 2 end: [5.61104768576663;0.938437776728017;0.875260404602983]
Robot Arm 1 togoal: 19.0727871709327

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986487e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.692041e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-4.33782063484892;-5.76258479375882;-7.14571026184835]
Robot Arm 1 end: [-6.22419314266208;-6.77454616024046;0.306596282151043]
Robot Arm 1 togoal: 23.3684534876052
Mobile Platform 2 Pose: [-0.815283164991972;5.31185948109739;1.59635478199564]
Robot Arm 2 end: [5.50676303357728;0.849615799827565;0.980479728794161]
Robot Arm 1 togoal: 18.9331990151419

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974908e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.715577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-4.34866345907983;-5.75563333005738;-7.14669550334333]
Robot Arm 1 end: [-6.26001759298335;-6.76801362094971;0.356495751863864]
Robot Arm 1 togoal: 23.386118169983
Mobile Platform 2 Pose: [-0.399173638355034;5.08421854186835;1.49301220611261]
Robot Arm 2 end: [5.14232383237649;0.845846018208205;1.15192147749413]
Robot Arm 1 togoal: 18.6295079336792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995958e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999160e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-4.37071515042882;-5.74140865744064;-7.14825776208326]
Robot Arm 1 end: [-6.28305399112415;-6.75823862008692;0.458521861672025]
Robot Arm 1 togoal: 23.3902815015139
Mobile Platform 2 Pose: [-0.0962268128748373;4.92527170888651;1.44465351398939]
Robot Arm 2 end: [4.85648275255082;0.864715445889053;1.20775352239084]
Robot Arm 1 togoal: 18.40802120959

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-4.38457212182543;-5.73227523510557;-7.14940718377184]
Robot Arm 1 end: [-6.27593362512273;-6.78957220471601;0.388285133805363]
Robot Arm 1 togoal: 23.4110826993301
Mobile Platform 2 Pose: [0.187143904283593;4.72739263093415;1.29186650371444]
Robot Arm 2 end: [4.65707541552367;0.892160834812785;1.32415213782557]
Robot Arm 1 togoal: 18.2621952806507

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-4.42285224075019;-5.70783826890814;-7.15154711750238]
Robot Arm 1 end: [-6.24389133428781;-6.8158666236441;0.351604155265049]
Robot Arm 1 togoal: 23.4095036817533
Mobile Platform 2 Pose: [-0.0808834173876817;4.81663540511061;1.23399904720123]
Robot Arm 2 end: [4.74563586940909;0.654529052475784;1.20793440297419]
Robot Arm 1 togoal: 18.1948008120808

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994831e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-4.46735688121354;-5.67840361403418;-7.15441916325329]
Robot Arm 1 end: [-6.20308041868799;-6.84881738465066;0.327177987273006]
Robot Arm 1 togoal: 23.406146058543
Mobile Platform 2 Pose: [0.243521784611132;4.61009595794236;1.09313934110038]
Robot Arm 2 end: [4.53512878668315;0.684856714210436;1.31809307180073]
Robot Arm 1 togoal: 18.0409895258044

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994565e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-4.50639970813754;-5.651546730958;-7.15782659834041]
Robot Arm 1 end: [-6.16331284549265;-6.89765179555164;0.30625292924668]
Robot Arm 1 togoal: 23.4149296131951
Mobile Platform 2 Pose: [-0.019530790288206;4.69760091544587;1.02807629178204]
Robot Arm 2 end: [4.62151271738816;0.442493810327925;1.19881724127388]
Robot Arm 1 togoal: 17.9704789266592

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995636e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-4.53415023698255;-5.63185594977396;-7.16127372358779]
Robot Arm 1 end: [-6.13434380041506;-6.9583716647601;0.290596437493947]
Robot Arm 1 togoal: 23.4396525639088
Mobile Platform 2 Pose: [0.306863872896334;4.48906170929572;0.89019446011985]
Robot Arm 2 end: [4.41454595135901;0.481829204486295;1.31809307179975]
Robot Arm 1 togoal: 17.8238337536394

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997432e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-4.55901737517262;-5.61530331578758;-7.16426188317778]
Robot Arm 1 end: [-6.13007193093918;-7.03769782099191;0.289515871598313]
Robot Arm 1 togoal: 23.4942316472621
Mobile Platform 2 Pose: [0.0464384884506838;4.57576050044965;0.823889324435302]
Robot Arm 2 end: [4.50034658178324;0.238219624568653;1.19881724127018]
Robot Arm 1 togoal: 17.7534501943357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-4.58390505524666;-5.59969778045328;-7.16668162657551]
Robot Arm 1 end: [-6.13361384414755;-7.11700272588927;0.312488843092087]
Robot Arm 1 togoal: 23.553041782465
Mobile Platform 2 Pose: [0.372993564335686;4.36750554343249;0.685578540758253]
Robot Arm 2 end: [4.29346116271884;0.277127552424053;1.31809307179992]
Robot Arm 1 togoal: 17.6057818332631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999163e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-4.60648930855139;-5.58628245744128;-7.16841400770094]
Robot Arm 1 end: [-6.13973270665574;-7.18726289895258;0.348019170620641]
Robot Arm 1 togoal: 23.6065427758089
Mobile Platform 2 Pose: [0.114266764943096;4.45364403946382;0.618715293310749]
Robot Arm 2 end: [4.37892350448693;0.0329567269011901;1.19881724127586]
Robot Arm 1 togoal: 17.5361758534116

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998605e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-4.62335239944379;-5.57702200465578;-7.16931851237504]
Robot Arm 1 end: [-6.15364915144484;-7.24321893723845;0.386088983188563]
Robot Arm 1 togoal: 23.6549572656894
Mobile Platform 2 Pose: [0.440920795815142;4.24548507906293;0.480260075971285]
Robot Arm 2 end: [4.17192499060395;0.0717215911535399;1.31809307179989]
Robot Arm 1 togoal: 17.3874726447096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.61575321062558;-5.58166146409757;-7.16896711297871]
Robot Arm 1 end: [-6.18478756087109;-7.27638863538869;0.432874106306202]
Robot Arm 1 togoal: 23.698206063583
Mobile Platform 2 Pose: [0.183239634376977;4.33122493591967;0.413284436948967]
Robot Arm 2 end: [4.25720959289431;-0.172563653828599;1.19881724127564]
Robot Arm 1 togoal: 17.3190742317188

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.60213373676634;-5.59061247893673;-7.16733566088601]
Robot Arm 1 end: [-6.23180271924681;-7.28894279461804;0.477591322022484]
Robot Arm 1 togoal: 23.7374738365607
Mobile Platform 2 Pose: [0.509978246780331;4.12310191055481;0.27477520867539]
Robot Arm 2 end: [4.05003427839401;-0.133851639832506;1.31809307180126]
Robot Arm 1 togoal: 17.1693190869759

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.58254774221678;-5.6039759737683;-7.16332411136939]
Robot Arm 1 end: [-6.29392517400472;-7.28041007493999;0.513029486139327]
Robot Arm 1 togoal: 23.7722648161879
Mobile Platform 2 Pose: [0.253152731289232;4.20848633522075;0.207827559047667]
Robot Arm 2 end: [4.13518590704606;-0.378110410115583;1.19881724126701]
Robot Arm 1 togoal: 17.1022817669662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-4.56860417877223;-5.61569025692917;-7.15467801080142]
Robot Arm 1 end: [-6.32962815363531;-7.32202908338791;0.504276448886871]
Robot Arm 1 togoal: 23.8273618762364
Mobile Platform 2 Pose: [0.579980941536033;4.00039875111113;0.0692650917496183]
Robot Arm 2 end: [3.92783042447409;-0.339450724128811;1.31809307180722]
Robot Arm 1 togoal: 16.9514555087742

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-4.5478860261574;-5.6333816833507;-7.13989153622937]
Robot Arm 1 end: [-6.34836719128719;-7.37464188730371;0.467610482102293]
Robot Arm 1 togoal: 23.8800527639622
Mobile Platform 2 Pose: [0.324045706715262;4.08541395817325;0.00234817901968437]
Robot Arm 2 end: [4.01283857594092;-0.583680037335528;1.19881724127265]
Robot Arm 1 togoal: 16.8858084889256

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-4.49305617136054;-5.67449985907234;-7.11986232291634]
Robot Arm 1 end: [-6.38878648019668;-7.39089838023124;0.439692542337135]
Robot Arm 1 togoal: 23.9208046391212
Mobile Platform 2 Pose: [0.650968795942468;3.8773612636322;-0.136266669747699]
Robot Arm 2 end: [3.80529937971788;-0.545072081986336;1.31809307180041]
Robot Arm 1 togoal: 16.7338914816603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-4.40027971096439;-5.74269745567495;-7.09826822580984]
Robot Arm 1 end: [-6.46992958156466;-7.36548783072928;0.43559025399578]
Robot Arm 1 togoal: 23.9582316195838
Mobile Platform 2 Pose: [0.395960137193559;3.96199281715936;-0.203149938241472]
Robot Arm 2 end: [3.89015304202668;-0.789268785563455;1.19881724126313]
Robot Arm 1 togoal: 16.6696647466452

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-4.26902340652057;-5.83855413159045;-7.072950715837]
Robot Arm 1 end: [-6.62058944075917;-7.16236399189289;0.456661590479974]
Robot Arm 1 togoal: 23.9149231108627
Mobile Platform 2 Pose: [0.722983671357604;3.75397440068902;-0.34181622258684]
Robot Arm 2 end: [3.6824264001092;-0.750711886284837;1.31809307180565]
Robot Arm 1 togoal: 16.5166368860742

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989423e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-4.18493444991129;-5.9157193120439;-6.99310463109419]
Robot Arm 1 end: [-6.70322932627381;-6.89559111616978;0.46698420566848]
Robot Arm 1 togoal: 23.7817118472491
Mobile Platform 2 Pose: [0.468939642442131;3.83820717686238;-0.408662768441692]
Robot Arm 2 end: [3.76712471827203;-0.994861339894639;1.19883478797591]
Robot Arm 1 togoal: 16.4538760090144

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-4.34245488028718;-5.83485433116272;-6.93513728076559]
Robot Arm 1 end: [-6.57066401436391;-6.87281662898653;0.289395313142531]
Robot Arm 1 togoal: 23.6811253317284
Mobile Platform 2 Pose: [0.796066416759696;3.63022755282851;-0.547387229125832]
Robot Arm 2 end: [3.55921055438325;-0.956357719557911;1.31810486571872]
Robot Arm 1 togoal: 16.2997184942015

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-4.39516628812933;-5.82627192821094;-6.81907565122321]
Robot Arm 1 end: [-6.55242323094937;-6.73075711347217;0.271300308100835]
Robot Arm 1 togoal: 23.5682561452137
Mobile Platform 2 Pose: [0.543032801425067;3.71404397731907;-0.614194984196421]
Robot Arm 2 end: [3.64388137101222;-1.20031356356298;1.1991002423745]
Robot Arm 1 togoal: 16.2386452544486

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-4.53825787158912;-5.75687287069181;-6.71538550748989]
Robot Arm 1 end: [-6.45376996655762;-6.63554384169185;0.207924686342109]
Robot Arm 1 togoal: 23.4348465282063
Mobile Platform 2 Pose: [0.870225786980506;3.5061759332784;-0.753086583196976]
Robot Arm 2 end: [3.43583005530835;-1.16190469286866;1.318283228176]
Robot Arm 1 togoal: 16.0833625844424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-4.71039138960864;-5.66362977858396;-6.6101418089039]
Robot Arm 1 end: [-6.30043279791227;-6.52238079483962;0.124448903967718]
Robot Arm 1 togoal: 23.2517425474472
Mobile Platform 2 Pose: [0.618326433850499;3.58953529248459;-0.819869392610587]
Robot Arm 2 end: [3.52022870453021;-1.40492968612596;1.20031411836195]
Robot Arm 1 togoal: 16.0241897566749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-4.8979114683764;-5.55788262728562;-6.48919351382592]
Robot Arm 1 end: [-6.2070403439483;-6.42331420014699;0.137526310299735]
Robot Arm 1 togoal: 23.1150904276235
Mobile Platform 2 Pose: [0.944397867644372;3.38213789833949;-0.95946280545845]
Robot Arm 2 end: [3.31267290300567;-1.36701820789465;1.31913337079589]
Robot Arm 1 togoal: 15.8680932875135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-5.11328944942742;-5.42933614798019;-6.41931090483768]
Robot Arm 1 end: [-5.99975599941061;-6.31680058323527;0.0583314498524938]
Robot Arm 1 togoal: 22.8991172886933
Mobile Platform 2 Pose: [0.693413174649347;3.46512490319785;-1.02613204919103]
Robot Arm 2 end: [3.39691177103047;-1.60849890773057;1.20329994391075]
Robot Arm 1 togoal: 15.8112450144817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-5.37034751431415;-5.27726358133253;-6.33705216656469]
Robot Arm 1 end: [-5.71247438909055;-6.24098243452831;-0.0414998074796301]
Robot Arm 1 togoal: 22.6515511061573
Mobile Platform 2 Pose: [1.01663896870583;3.25885878792142;-1.16739170268812]
Robot Arm 2 end: [3.19082650418157;-1.57172820299289;1.32121280941708]
Robot Arm 1 togoal: 15.6548262292868

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-5.5647815409527;-5.14888386331317;-6.17635040968723]
Robot Arm 1 end: [-5.4757933209249;-6.0705920413171;-0.0874463083599723]
Robot Arm 1 togoal: 22.3685316236293
Mobile Platform 2 Pose: [0.76642630898318;3.34154051436262;-1.23388950859115]
Robot Arm 2 end: [3.27473671714664;-1.81082793821825;1.20914159811858]
Robot Arm 1 togoal: 15.6005906820816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-5.59897732883346;-5.10694533123035;-5.98143645719899]
Robot Arm 1 end: [-5.50953260387822;-5.83049956562923;-0.0373565343926038]
Robot Arm 1 togoal: 22.2166332547583
Mobile Platform 2 Pose: [1.08358290061252;3.13759203528886;-1.37847518521059]
Robot Arm 2 end: [3.07185487562974;-1.7763547863521;1.32525733196879]
Robot Arm 1 togoal: 15.4447416838731

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.216534e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.532310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-5.84848394034402;-4.9771283838776;-5.88958140544324]
Robot Arm 1 end: [-5.30702926011406;-5.58721144910283;-0.000622993038828124]
Robot Arm 1 togoal: 21.899352548922
Mobile Platform 2 Pose: [0.833780743981369;3.22011192268195;-1.44473317299252]
Robot Arm 2 end: [3.15398639528022;-2.02174973560585;1.20914159812017]
Robot Arm 1 togoal: 15.3875628684774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.072960e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.670493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-5.92982347695943;-4.97717427116576;-5.76266064797888]
Robot Arm 1 end: [-5.30539238760758;-5.38362245894022;0.0400168055888442]
Robot Arm 1 togoal: 21.7509583080832
Mobile Platform 2 Pose: [1.20528334778528;2.97834335188044;-1.59708554986536]
Robot Arm 2 end: [2.80247930407772;-1.95496274216505;1.32789745900585]
Robot Arm 1 togoal: 15.1216199331497

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.468585e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-5.83076477506907;-5.04863055159236;-5.73590450917]
Robot Arm 1 end: [-5.53731550831679;-5.37548391948205;0.117228101371175]
Robot Arm 1 togoal: 21.9039780520939
Mobile Platform 2 Pose: [0.887209781753003;3.02963457606852;-1.73607184225112]
Robot Arm 2 end: [3.03678621353262;-2.25108296776993;1.24417459969451]
Robot Arm 1 togoal: 15.16839286978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.304499e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-5.74916240900246;-5.15047183609796;-5.65722303199159]
Robot Arm 1 end: [-5.77117838959352;-5.28361177651081;0.265350428884655]
Robot Arm 1 togoal: 21.9976251576602
Mobile Platform 2 Pose: [0.92789612473663;2.91818583692365;-1.91479826146993]
Robot Arm 2 end: [2.89428050939395;-2.47922837345026;1.17828080340258]
Robot Arm 1 togoal: 14.9312401414407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.323095e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.732297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-5.9214732363279;-5.13356002221968;-5.56961283147851]
Robot Arm 1 end: [-5.66824223312266;-5.15886361726259;0.233254039046345]
Robot Arm 1 togoal: 21.8390304387014
Mobile Platform 2 Pose: [0.629963938121135;2.99990922358655;-1.96723031467286]
Robot Arm 2 end: [3.0967593421472;-2.65244217288914;1.0531832065836]
Robot Arm 1 togoal: 15.0243323925245

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.159445e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.782835e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-5.88213902821072;-5.2222917666395;-5.50459222572287]
Robot Arm 1 end: [-5.81192064275294;-5.15129659394304;0.27673057824365]
Robot Arm 1 togoal: 21.9346034676778
Mobile Platform 2 Pose: [0.313029805569212;3.08393227591081;-2.05343220442206]
Robot Arm 2 end: [3.32576968643087;-2.80504873038316;0.869218576071076]
Robot Arm 1 togoal: 15.1581093983564

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.471143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-5.85843877415317;-5.24288460188025;-5.50040391893593]
Robot Arm 1 end: [-5.81485830035928;-5.14083161411146;0.269000332230776]
Robot Arm 1 togoal: 21.9299345232815
Mobile Platform 2 Pose: [0.0619835210903459;3.14470601245666;-2.16112803190562]
Robot Arm 2 end: [3.383434864558;-2.96515377397165;0.672480783080313]
Robot Arm 1 togoal: 15.1434928289089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-5.84960249631595;-5.24603031068694;-5.50201507082874]
Robot Arm 1 end: [-5.77778222560834;-5.09790265612669;0.269353507596664]
Robot Arm 1 togoal: 21.8735387058095
Mobile Platform 2 Pose: [-0.119793740894465;3.13795316214322;-2.32476248491146]
Robot Arm 2 end: [3.34314666816317;-3.13338818507667;0.46421652109349]
Robot Arm 1 togoal: 15.0434940139001

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-5.83778039976169;-5.24739695061404;-5.5064193928644]
Robot Arm 1 end: [-5.72422012413726;-5.03895111502187;0.267943262069593]
Robot Arm 1 togoal: 21.7942951329198
Mobile Platform 2 Pose: [0.152694376991876;2.93702744697593;-2.45667633098772]
Robot Arm 2 end: [3.27695694400946;-3.22812542706691;0.664598393281294]
Robot Arm 1 togoal: 14.9288075423528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-5.82835141725017;-5.24666500180091;-5.51210464003881]
Robot Arm 1 end: [-5.64744753595883;-4.97866967518774;0.266078404315728]
Robot Arm 1 togoal: 21.6974189211594
Mobile Platform 2 Pose: [0.485551374010199;2.7413870601557;-2.59575372035093]
Robot Arm 2 end: [3.08797664547494;-3.33008650454712;0.751127491497769]
Robot Arm 1 togoal: 14.7097104794306

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-5.80664647971627;-5.25130932281752;-5.52277334245738]
Robot Arm 1 end: [-5.54155147185054;-4.92652265109713;0.258247282426628]
Robot Arm 1 togoal: 21.5855378675546
Mobile Platform 2 Pose: [0.723032766360221;2.6271343033875;-2.6368108775689]
Robot Arm 2 end: [2.8334467343224;-3.3514666619043;0.942199191275721]
Robot Arm 1 togoal: 14.4649701350068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999608e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-5.74872080198456;-5.27108973622139;-5.564104680625]
Robot Arm 1 end: [-5.43940275298735;-4.960626063568;0.205798652247775]
Robot Arm 1 togoal: 21.5389239324529
Mobile Platform 2 Pose: [1.02478291335208;2.4689373848842;-2.70735356539269]
Robot Arm 2 end: [2.5189388449591;-3.38682807078908;1.01691507642047]
Robot Arm 1 togoal: 14.1672853567403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998282e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-5.9624436540889;-5.14270263136501;-5.60548718229252]
Robot Arm 1 end: [-5.26733239384561;-4.94208319060689;0.282063705566114]
Robot Arm 1 togoal: 21.3984170358662
Mobile Platform 2 Pose: [1.30344979308009;2.30846565429478;-2.80169335577862]
Robot Arm 2 end: [2.27185146453184;-3.38640403454638;1.13146039764236]
Robot Arm 1 togoal: 13.9459575169151

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-6.14986956409535;-5.03145511534815;-5.63769634655915]
Robot Arm 1 end: [-5.12837164461538;-4.93309904511555;0.368380945472699]
Robot Arm 1 togoal: 21.2883443788777
Mobile Platform 2 Pose: [1.34675296190684;2.22332488321073;-2.9626007813015]
Robot Arm 2 end: [2.2392896975526;-3.50159130592;1.19691254572731]
Robot Arm 1 togoal: 13.8612611451458

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-6.29145780489013;-4.94799016149915;-5.64649563396014]
Robot Arm 1 end: [-4.98404278683509;-4.91126554073266;0.434837533871772]
Robot Arm 1 togoal: 21.1671237339127
Mobile Platform 2 Pose: [1.45098281349866;2.10832277058636;-3.16232746628783]
Robot Arm 2 end: [2.05438466133798;-3.74537163029241;1.13177437490803]
Robot Arm 1 togoal: 13.5860245176387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-6.45143656644205;-4.85190543995377;-5.63806295182855]
Robot Arm 1 end: [-4.89621918055311;-4.88513156282669;0.532920832258458]
Robot Arm 1 togoal: 21.081764346455
Mobile Platform 2 Pose: [1.47861063252517;2.0065980798558;-3.32306163693702]
Robot Arm 2 end: [2.01821401563475;-3.86365197939981;1.19792863390685]
Robot Arm 1 togoal: 13.4980224574977

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-6.60025495933712;-4.75850343658519;-5.61615889939495]
Robot Arm 1 end: [-4.78022573681393;-4.85274870478408;0.638959786601672]
Robot Arm 1 togoal: 20.9722962943187
Mobile Platform 2 Pose: [1.50656732853433;1.89568863442137;-3.53077870780761]
Robot Arm 2 end: [1.88872539241592;-4.11644943563233;1.13155423664214]
Robot Arm 1 togoal: 13.2706302300359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.336327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-6.70361551644281;-4.67076828168236;-5.54513797459545]
Robot Arm 1 end: [-4.67520454894512;-4.78933990306117;0.762560730402447]
Robot Arm 1 togoal: 20.8485355228449
Mobile Platform 2 Pose: [1.67334547231775;1.72264496396673;-3.68362408623747]
Robot Arm 2 end: [1.60887573243549;-4.20813727622239;1.18194794619184]
Robot Arm 1 togoal: 12.977928412401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972666e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-6.59886105943656;-4.69970018025779;-5.47757872743503]
Robot Arm 1 end: [-4.5769575659588;-4.69722526523532;0.681089968721598]
Robot Arm 1 togoal: 20.7171805812827
Mobile Platform 2 Pose: [1.73945713329136;1.58601594115744;-3.82100586726638]
Robot Arm 2 end: [1.39765957477098;-4.33977129258164;1.1517224283783]
Robot Arm 1 togoal: 12.7311114490273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.532533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.255965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-6.43299736844251;-4.73668076331732;-5.37099640834173]
Robot Arm 1 end: [-4.45620556255163;-4.63079162320767;0.481087524020067]
Robot Arm 1 togoal: 20.5942555278449
Mobile Platform 2 Pose: [1.57470548084501;1.59233040436434;-3.87555571645448]
Robot Arm 2 end: [1.43382155734175;-4.45920301545858;1.11966680708035]
Robot Arm 1 togoal: 12.7119540181978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.578820e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-6.6700275734968;-4.54669575473795;-5.2671821555024]
Robot Arm 1 end: [-4.42583733788923;-4.48702573764426;0.692073068006457]
Robot Arm 1 togoal: 20.4613249199799
Mobile Platform 2 Pose: [1.74762917567173;1.40044327436337;-4.03580342895199]
Robot Arm 2 end: [1.19784683161665;-4.54105899445956;1.17706189710453]
Robot Arm 1 togoal: 12.4623474508193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006502e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-6.45992812918412;-4.61176976718721;-5.16495930283601]
Robot Arm 1 end: [-4.33814895683114;-4.42204122401559;0.479820225485991]
Robot Arm 1 togoal: 20.3632473809641
Mobile Platform 2 Pose: [1.54069540876069;1.39125655945155;-4.1460714740554]
Robot Arm 2 end: [1.21639788379205;-4.7234530620931;1.13878920995429]
Robot Arm 1 togoal: 12.4014359718339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.877752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-6.56059419762923;-4.46792019115509;-5.01025866992898]
Robot Arm 1 end: [-4.24323674462758;-4.274033133989;0.456181360113847]
Robot Arm 1 togoal: 20.1928614517501
Mobile Platform 2 Pose: [1.88878988589915;1.16091843231993;-4.24325350871484]
Robot Arm 2 end: [0.981984155638958;-4.6732476815219;1.24901907726096]
Robot Arm 1 togoal: 12.2086952570474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.439069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-6.82909206639139;-4.29203177084559;-4.96011989495266]
Robot Arm 1 end: [-4.1896110310723;-4.20236034703589;0.592218705727271]
Robot Arm 1 togoal: 20.0976300924457
Mobile Platform 2 Pose: [1.6110776664904;1.19296475286783;-4.34981896628393]
Robot Arm 2 end: [1.06914665178957;-4.89312503715162;1.18865270301445]
Robot Arm 1 togoal: 12.1949429769395

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.324741e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-7.02718157476076;-4.16795871595995;-4.93577508788743]
Robot Arm 1 end: [-4.06537992304695;-4.18916122926229;0.76820395878169]
Robot Arm 1 togoal: 19.9933466595597
Mobile Platform 2 Pose: [1.92073122925483;1.0138000218348;-4.4145418456413]
Robot Arm 2 end: [0.802342595653741;-4.8647232652914;1.25885729358377]
Robot Arm 1 togoal: 11.9637114431284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.625757e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-7.21906951576507;-4.036895912213;-4.89711720802819]
Robot Arm 1 end: [-3.94306336268841;-4.18769630421495;0.898276268210753]
Robot Arm 1 togoal: 19.901947824339
Mobile Platform 2 Pose: [1.91191188466101;0.873476350119388;-4.6214458503302]
Robot Arm 2 end: [0.724436220314184;-4.90120786830739;1.34532686100803]
Robot Arm 1 togoal: 11.8761136108392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.245134e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-7.38727765314174;-3.92076438796048;-4.86614987317041]
Robot Arm 1 end: [-3.78677057977813;-4.23740098581705;1.05185337354462]
Robot Arm 1 togoal: 19.8241955298946
Mobile Platform 2 Pose: [1.60421366337629;0.903717153710111;-4.76458043937452]
Robot Arm 2 end: [0.781729851498342;-4.9811938877394;1.38878988752488]
Robot Arm 1 togoal: 11.893342377019

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.264309e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-7.55183249741596;-3.79741617202448;-4.83272609487189]
Robot Arm 1 end: [-3.58984474603362;-4.31259631049304;1.17238896439711]
Robot Arm 1 togoal: 19.7397016953798
Mobile Platform 2 Pose: [1.57096315783295;0.688505637662762;-4.88629706176617]
Robot Arm 2 end: [0.538497213428217;-5.16303233118121;1.34267411493925]
Robot Arm 1 togoal: 11.5968952684723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.866353e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-7.71900317716422;-3.65843772358277;-4.79815688229114]
Robot Arm 1 end: [-3.38356504463589;-4.45941696781497;1.30337329471701]
Robot Arm 1 togoal: 19.703855301336
Mobile Platform 2 Pose: [1.16896391544359;0.790814933522398;-4.91313637097517]
Robot Arm 2 end: [0.572027948244712;-5.35052850332691;1.26214332895844]
Robot Arm 1 togoal: 11.552158539121

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.379485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-7.66675721365241;-3.59104316011033;-4.74030552261638]
Robot Arm 1 end: [-3.20200052376118;-4.40360074223859;1.26411930347127]
Robot Arm 1 togoal: 19.540279421325
Mobile Platform 2 Pose: [1.14839054270215;0.575703272165557;-5.04052150805289]
Robot Arm 2 end: [0.327033685675279;-5.34734475313793;1.28963161742243]
Robot Arm 1 togoal: 11.3290933847189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.136373e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-7.61265662256685;-3.51800498111325;-4.67067670141554]
Robot Arm 1 end: [-3.05706957668125;-4.33108968271289;1.23741965690422]
Robot Arm 1 togoal: 19.3893686136306
Mobile Platform 2 Pose: [0.832771204625635;0.527762622498224;-5.10188428537147]
Robot Arm 2 end: [0.362184709637776;-5.52838590783775;1.25036233861761]
Robot Arm 1 togoal: 11.2890854053654
Step 135:
Mobile Platform 1 Pose: [-7.48991980584833;-3.47016499114773;-4.5785131255292]
Robot Arm 1 end: [-2.87380697709494;-4.29016377071932;1.15763770295881]
Robot Arm 1 togoal: 19.2373521784155
Mobile Platform 2 Pose: [0.577747243094553;0.597806911167618;-5.11417310118489]
Robot Arm 2 end: [0.454905594259459;-5.68987937468601;1.1532734167609]
Robot Arm 1 togoal: 11.3144780537882
Step 136:
Mobile Platform 1 Pose: [-7.4454191763757;-3.36920147859627;-4.49854863319644]
Robot Arm 1 end: [-2.73732642857937;-4.26288919548343;1.12104210848783]
Robot Arm 1 togoal: 19.1266687916586
Mobile Platform 2 Pose: [0.302017588260518;0.674116467396964;-5.14770730737601]
Robot Arm 2 end: [0.581965511856353;-5.83770439756809;1.05112266078797]
Robot Arm 1 togoal: 11.3808384352844
Step 137:
Mobile Platform 1 Pose: [-7.47262183525666;-3.15977004660721;-4.4061875952863]
Robot Arm 1 end: [-2.59658398666763;-4.17970174071344;1.1803146743444]
Robot Arm 1 togoal: 18.969816448239
Mobile Platform 2 Pose: [0.0164741917675718;0.747926392318011;-5.20833863101353]
Robot Arm 2 end: [0.645354244145509;-5.9791808395503;0.93313055877268]
Robot Arm 1 togoal: 11.3945664324851
Step 138:
Mobile Platform 1 Pose: [-7.6162394801144;-2.92083911582448;-4.33262122854514]
Robot Arm 1 end: [-2.37841401106192;-3.97858500946661;1.10456321782364]
Robot Arm 1 togoal: 18.676171767514
Mobile Platform 2 Pose: [-0.217031253877734;0.760770772054788;-5.34155702789531]
Robot Arm 2 end: [0.589221871312866;-6.14540560807267;0.86560925209259]
Robot Arm 1 togoal: 11.2880049078042
Step 139:
Mobile Platform 1 Pose: [-7.43930403998343;-2.93759451905229;-4.28663239610803]
Robot Arm 1 end: [-2.39227604840081;-4.04564831923701;1.16521881601232]
Robot Arm 1 togoal: 18.7343353930246
Mobile Platform 2 Pose: [-0.015496803317744;0.56709012923293;-5.49963235359833]
Robot Arm 2 end: [0.50932271698272;-6.30674257271374;0.946947772457635]
Robot Arm 1 togoal: 11.1541718811322
Step 140:
Mobile Platform 1 Pose: [-7.70022212856327;-2.73084331479217;-4.2614985225222]
Robot Arm 1 end: [-2.18481403787546;-3.90387633212817;1.10780038705388]
Robot Arm 1 togoal: 18.4921119431194
Mobile Platform 2 Pose: [0.184741905964687;0.373211764215108;-5.65746511185478]
Robot Arm 2 end: [0.461173746360622;-6.35530014972463;1.12324518792787]
Robot Arm 1 togoal: 11.0850440703079
Step 141:
Mobile Platform 1 Pose: [-7.51142953410233;-2.72901784364848;-4.20393374845513]
Robot Arm 1 end: [-2.1868876612364;-3.95737095248195;1.1643390085336]
Robot Arm 1 togoal: 18.5325562682436
Mobile Platform 2 Pose: [0.0981118626080529;0.280888877813916;-5.87538059426117]
Robot Arm 2 end: [0.416821772396727;-6.4985486617962;1.22441374132011]
Robot Arm 1 togoal: 10.9935572368786
Step 142:
Mobile Platform 1 Pose: [-7.75962271994877;-2.54659486031992;-4.18680397394903]
Robot Arm 1 end: [-1.99758289726242;-3.82110077747946;1.11255919770735]
Robot Arm 1 togoal: 18.3066011582408
Mobile Platform 2 Pose: [0.64975508722546;0.0496257753349494;-5.97034046700651]
Robot Arm 2 end: [0.244133635347953;-6.39149982242075;1.35654022905753]
Robot Arm 1 togoal: 10.8623475587331
Step 143:
Mobile Platform 1 Pose: [-7.68171083604087;-2.40773500806999;-4.09829711741441]
Robot Arm 1 end: [-1.85727549516335;-3.86440885635754;1.16701801605258]
Robot Arm 1 togoal: 18.2467015745149
Mobile Platform 2 Pose: [1.39956905062729;-0.196672004793244;-6.01320541317392]
Robot Arm 2 end: [-0.0462311217795266;-6.3216141508164;1.43423201937021]
Robot Arm 1 togoal: 10.6120481538187
Step 144:
Mobile Platform 1 Pose: [-7.92176284141025;-2.1679530767672;-4.0538293685302]
Robot Arm 1 end: [-1.63257690609507;-3.71371205588919;1.09811745207991]
Robot Arm 1 togoal: 17.9878173627984
Mobile Platform 2 Pose: [2.00109218891327;-0.429233587274592;-6.10493932382862]
Robot Arm 2 end: [-0.331285294265388;-6.23506878431488;1.47781432430378]
Robot Arm 1 togoal: 10.3759633828534
Step 145:
Mobile Platform 1 Pose: [-7.80438198390496;-2.18685546476752;-4.02656883225207]
Robot Arm 1 end: [-1.6217618083842;-3.78951757953652;1.13701636100787]
Robot Arm 1 togoal: 18.037837621017
Mobile Platform 2 Pose: [2.00728066637875;-0.27734058813289;-5.90637317110979]
Robot Arm 2 end: [-0.309827251554725;-5.97650590349919;1.50185739367043]
Robot Arm 1 togoal: 10.4922980837691
Step 146:
Mobile Platform 1 Pose: [-7.85523691881129;-1.98776845610411;-3.94260382575129]
Robot Arm 1 end: [-1.40610021324005;-3.69548238716092;1.08272617532346]
Robot Arm 1 togoal: 17.8285569757316
Mobile Platform 2 Pose: [1.87380429610858;-0.524112882510024;-5.86633026002874]
Robot Arm 2 end: [-0.488662057164303;-6.08895529986392;1.47763404358361]
Robot Arm 1 togoal: 10.2841480306083
Step 147:
Mobile Platform 1 Pose: [-7.83572070791422;-1.86498645983481;-3.8674243390502]
Robot Arm 1 end: [-1.28262871396414;-3.74200355438861;1.15304314289524]
Robot Arm 1 togoal: 17.7841436295001
Mobile Platform 2 Pose: [2.35665659630944;-0.748889526872895;-5.97576270562441]
Robot Arm 2 end: [-0.635845854324155;-6.26010655504231;1.44751905429759]
Robot Arm 1 togoal: 10.0836295694059
Step 148:
Mobile Platform 1 Pose: [-8.06641652142154;-1.62960780907175;-3.82949600781427]
Robot Arm 1 end: [-1.03909031375295;-3.59520736069667;1.08690598313805]
Robot Arm 1 togoal: 17.5179797850954
Mobile Platform 2 Pose: [3.06532247672916;-0.971465005835816;-6.08960538727076]
Robot Arm 2 end: [-0.917824388084243;-6.2173598051786;1.48028270460778]
Robot Arm 1 togoal: 9.83849269895038
Step 149:
Mobile Platform 1 Pose: [-7.97102525063508;-1.63087835308815;-3.80454313193442]
Robot Arm 1 end: [-1.00075462233631;-3.67811704342728;1.0953944320561]
Robot Arm 1 togoal: 17.5581499086326
Mobile Platform 2 Pose: [2.31303551232655;-0.723188824907002;-6.06029774349649]
Robot Arm 2 end: [-0.602403109183126;-6.34877004445427;1.44495351217437]
Robot Arm 1 togoal: 10.0822661430294
Step 150:
Mobile Platform 1 Pose: [-8.04485028784497;-1.44064105623906;-3.7441317872924]
Robot Arm 1 end: [-0.825881286369922;-3.5726605489748;1.07348865298227]
Robot Arm 1 togoal: 17.3671266020024
Mobile Platform 2 Pose: [1.87791639824982;-0.728646924492472;-6.1908334774941]
Robot Arm 2 end: [-0.54100072395834;-6.58833566819364;1.36919942745468]
Robot Arm 1 togoal: 10.0565980446124
Step 151:
Mobile Platform 1 Pose: [-8.03226255619638;-1.32125950818624;-3.68184822183954]
Robot Arm 1 end: [-0.720824930556214;-3.63159844374002;1.14372981847545]
Robot Arm 1 togoal: 17.3464375625913
Mobile Platform 2 Pose: [1.44887768499244;-0.564833070929776;-6.20420669677735]
Robot Arm 2 end: [-0.441774118747268;-6.79514872135275;1.25812367031482]
Robot Arm 1 togoal: 10.0846347321063
Step 152:
Mobile Platform 1 Pose: [-8.22566309832606;-1.10104476925037;-3.64846273829454]
Robot Arm 1 end: [-0.523922915886757;-3.49808242382205;1.12753911011193]
Robot Arm 1 togoal: 17.1203386103106
Mobile Platform 2 Pose: [1.17960713052379;-0.465749542006306;-6.22433103017804]
Robot Arm 2 end: [-0.425870658751982;-6.92201923816443;1.16781333556375]
Robot Arm 1 togoal: 10.0629349115013
Step 153:
Mobile Platform 1 Pose: [-8.1509981267811;-1.08144995332298;-3.62343240416259]
Robot Arm 1 end: [-0.489590468445164;-3.5774849306079;1.14719149298004]
Robot Arm 1 togoal: 17.1615657221741
Mobile Platform 2 Pose: [0.87791695129865;-0.353383289677731;-6.24593427466552]
Robot Arm 2 end: [-0.409389268839978;-6.98827691687892;1.09648471228551]
Robot Arm 1 togoal: 10.0613370560552
Step 154:
Mobile Platform 1 Pose: [-8.30391077031074;-0.871319786826366;-3.58374588598404]
Robot Arm 1 end: [-0.309351628852715;-3.43586636422684;1.138125618504]
Robot Arm 1 togoal: 16.9396525571423
Mobile Platform 2 Pose: [0.551913514361743;-0.227776030622685;-6.26567301145144]
Robot Arm 2 end: [-0.412026039988799;-7.00609921252158;1.0275565033007]
Robot Arm 1 togoal: 10.0566482024356
Step 155:
Mobile Platform 1 Pose: [-8.23087670391532;-0.829296835419602;-3.54597460020959]
Robot Arm 1 end: [-0.27018729755279;-3.49977290294817;1.17979049715151]
Robot Arm 1 togoal: 16.9657607988213
Mobile Platform 2 Pose: [0.208060418480007;-0.103051833499159;-6.30511459937308]
Robot Arm 2 end: [-0.414797691444262;-6.96200136827251;1.07138892131067]
Robot Arm 1 togoal: 10.0651720851857
Step 156:
Mobile Platform 1 Pose: [-8.43655039248868;-0.609567980156772;-3.51473746265835]
Robot Arm 1 end: [-0.0654896634126239;-3.37787151555895;1.16058124758332]
Robot Arm 1 togoal: 16.7454898443779
Mobile Platform 2 Pose: [0.0884290343767502;-0.155172184621893;-6.52134925613349]
Robot Arm 2 end: [-0.531759871330684;-7.04452899271455;1.16010588751136]
Robot Arm 1 togoal: 9.92535261688358
Step 157:
Mobile Platform 1 Pose: [-8.37684352994266;-0.590237432687356;-3.49323946181649]
Robot Arm 1 end: [-0.0316697449065618;-3.45260867316951;1.17490617323811]
Robot Arm 1 togoal: 16.7847209962284
Mobile Platform 2 Pose: [0.581317765367481;-0.309405159918856;-6.52118020378236]
Robot Arm 2 end: [-0.593525574483625;-7.16223337575908;1.06024161441051]
Robot Arm 1 togoal: 9.83600421011705
Step 158:
Mobile Platform 1 Pose: [-8.50692801664805;-0.389753596231231;-3.45062546111354]
Robot Arm 1 end: [0.15002234559957;-3.31151477981744;1.16447458766627]
Robot Arm 1 togoal: 16.5633811733935
Mobile Platform 2 Pose: [0.595692642457621;-0.315652533994355;-6.51874205733994]
Robot Arm 2 end: [-0.5015106143429;-7.28104560033342;0.91535262879176]
Robot Arm 1 togoal: 9.89852627289327
Step 159:
Mobile Platform 1 Pose: [-8.45492672559568;-0.312971564334388;-3.40056070122892]
Robot Arm 1 end: [0.198745956515709;-3.35677685611711;1.22658527470965]
Robot Arm 1 togoal: 16.5696936857472
Mobile Platform 2 Pose: [0.712383861297858;-0.364872906330651;-6.5216279666904]
Robot Arm 2 end: [-0.422125539519246;-7.36333617727309;0.756220463255965]
Robot Arm 1 togoal: 9.96356176448105
Step 160:
Mobile Platform 1 Pose: [-8.69926778206729;-0.0918072108152153;-3.37794452796071]
Robot Arm 1 end: [0.420518386677331;-3.2615510391368;1.20149441949025]
Robot Arm 1 togoal: 16.362679724287
Mobile Platform 2 Pose: [0.875506821192765;-0.435002798291118;-6.52468830646421]
Robot Arm 2 end: [-0.350597159281732;-7.38047509259669;0.741510454800299]
Robot Arm 1 togoal: 10.028982494453
Step 161:
Mobile Platform 1 Pose: [-8.6542245402201;-0.081428147741345;-3.36466543501142]
Robot Arm 1 end: [0.44210917715398;-3.33555283711166;1.21270985509017]
Robot Arm 1 togoal: 16.4099143588956
Mobile Platform 2 Pose: [1.02127686655246;-0.496418681888155;-6.52568631940731]
Robot Arm 2 end: [-0.27625218589614;-7.3536561747366;0.810992845811308]
Robot Arm 1 togoal: 10.1024015636282
Step 162:
Mobile Platform 1 Pose: [-8.66906758410136;0.0684864087123282;-3.31673281315458]
Robot Arm 1 end: [0.590926383925402;-3.37579040600144;1.19629838503337]
Robot Arm 1 togoal: 16.35689048801
Mobile Platform 2 Pose: [1.18172141424679;-0.562670675237197;-6.52763298233158]
Robot Arm 2 end: [-0.224577485097325;-7.27629890518731;0.944359901087016]
Robot Arm 1 togoal: 10.1641500777204
Step 163:
Mobile Platform 1 Pose: [-8.83965767102782;0.233865499290553;-3.3039955323185]
Robot Arm 1 end: [0.700014287002525;-3.2773365755465;1.24978510588392]
Robot Arm 1 togoal: 16.2126789372112
Mobile Platform 2 Pose: [1.37399617259852;-0.639321273193673;-6.53416350507946]
Robot Arm 2 end: [-0.20571183108263;-7.18327837619501;1.04014667688504]
Robot Arm 1 togoal: 10.2026085566465
Step 164:
Mobile Platform 1 Pose: [-8.9659463091093;0.417155204655864;-3.27597620617679]
Robot Arm 1 end: [0.881358493787666;-3.16511351890309;1.23837506384746]
Robot Arm 1 togoal: 16.0171693422701
Mobile Platform 2 Pose: [1.59775113593463;-0.722918391188153;-6.55109300691075]
Robot Arm 2 end: [-0.232069595850598;-7.09116552243161;1.11051972399398]
Robot Arm 1 togoal: 10.2001115903301
Step 165:
Mobile Platform 1 Pose: [-8.96554735717404;0.52803385581721;-3.23232764017026]
Robot Arm 1 end: [0.951744567533967;-3.20151070872583;1.29307208638886]
Robot Arm 1 togoal: 16.0063350738874
Mobile Platform 2 Pose: [1.86803613188849;-0.813471365273461;-6.58729150026763]
Robot Arm 2 end: [-0.3353079969208;-7.03369213304886;1.18362663764689]
Robot Arm 1 togoal: 10.11528915363
Step 166:
Mobile Platform 1 Pose: [-9.24292002111814;0.776919028567553;-3.20874435987835]
Robot Arm 1 end: [1.20162529883006;-3.12803649191524;1.27133845758439]
Robot Arm 1 togoal: 15.8056657689895
Mobile Platform 2 Pose: [2.19370764869651;-0.928976586462969;-6.63178711708234]
Robot Arm 2 end: [-0.611535173746777;-7.03615743698442;1.30121361169269]
Robot Arm 1 togoal: 9.84763629449331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.703806e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-9.23619813553949;0.79316308605699;-3.20258818722294]
Robot Arm 1 end: [1.19417535322887;-3.19635214235458;1.29821092372547]
Robot Arm 1 togoal: 15.8661870828254
Mobile Platform 2 Pose: [2.58610150782482;-1.08880358407896;-6.68321326420004]
Robot Arm 2 end: [-0.907561770581132;-7.04294592510087;1.35898209138442]
Robot Arm 1 togoal: 9.56257557148635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.611510e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-9.16278928582152;0.871594517027823;-3.16269739180725]
Robot Arm 1 end: [1.22506553436799;-3.1985924424481;1.32404986973124]
Robot Arm 1 togoal: 15.8505869510185
Mobile Platform 2 Pose: [2.99834727020209;-1.26277920592401;-6.75126443361594]
Robot Arm 2 end: [-1.26774385555609;-7.02255736465959;1.41035094973329]
Robot Arm 1 togoal: 9.22657602966811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.504705e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-9.35616284606805;0.993010134912213;-3.16667936361947]
Robot Arm 1 end: [1.33516002919095;-3.15236610628405;1.34555828957782]
Robot Arm 1 togoal: 15.7510306236518
Mobile Platform 2 Pose: [3.57685295651083;-1.52845804836198;-6.91844155949267]
Robot Arm 2 end: [-1.6310831336489;-7.07303988226371;1.42439157612503]
Robot Arm 1 togoal: 8.86652120249416

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.402454e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-9.63939556673181;1.20096192714148;-3.16840554890068]
Robot Arm 1 end: [1.56348767482887;-3.10867676108332;1.32377598140625]
Robot Arm 1 togoal: 15.5900944177013
Mobile Platform 2 Pose: [3.96057009115274;-1.69125472441868;-6.9437685452918]
Robot Arm 2 end: [-1.91662211698855;-7.17669852037903;1.38698964568035]
Robot Arm 1 togoal: 8.56329305929988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-9.65556028027599;1.22394120350501;-3.16752782312336]
Robot Arm 1 end: [1.52405968245948;-3.1805967725058;1.35609281348618]
Robot Arm 1 togoal: 15.6715311927732
Mobile Platform 2 Pose: [4.20100499990407;-1.80048318828373;-6.97007167110513]
Robot Arm 2 end: [-2.0116258028817;-7.29890555481225;1.35904060159885]
Robot Arm 1 togoal: 8.43423169955272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-9.65389788024817;1.37760825098394;-3.14560217260285]
Robot Arm 1 end: [1.53553688400326;-3.16639361550455;1.38919520156796]
Robot Arm 1 togoal: 15.6530645301602
Mobile Platform 2 Pose: [4.40336047620791;-1.8969013414639;-6.99983192791465]
Robot Arm 2 end: [-2.07128031295049;-7.36357768336238;1.34429209962041]
Robot Arm 1 togoal: 8.35742449499413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-9.90133779242663;1.53345496905912;-3.15338696807137]
Robot Arm 1 end: [1.61938240609206;-3.16548939964904;1.41950478895814]
Robot Arm 1 togoal: 15.6068947476894
Mobile Platform 2 Pose: [4.58382880470498;-1.99089571406047;-7.02939046480051]
Robot Arm 2 end: [-2.11135206676042;-7.34640095761833;1.3711118417543]
Robot Arm 1 togoal: 8.32435105893043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-10.1296207422802;1.54359301461722;-3.17307795115816]
Robot Arm 1 end: [1.70221873625135;-3.15896978852636;1.38900983013233]
Robot Arm 1 togoal: 15.5572838632362
Mobile Platform 2 Pose: [4.78539925193343;-2.10806677489323;-7.06076064933014]
Robot Arm 2 end: [-2.18497236468339;-7.29103736640138;1.3988907778495]
Robot Arm 1 togoal: 8.27212464564758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.721419e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-10.2950166739502;1.62700990236073;-3.1854303292787]
Robot Arm 1 end: [1.84641073288606;-3.11881111171015;1.37384787080572]
Robot Arm 1 togoal: 15.4468727149107
Mobile Platform 2 Pose: [5.05646562244108;-2.28441429599044;-7.08769622237643]
Robot Arm 2 end: [-2.31984704712765;-7.16126610863974;1.42313742517359]
Robot Arm 1 togoal: 8.18857353688803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.218980e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-10.4359450821425;1.70481488696476;-3.19483538998762]
Robot Arm 1 end: [1.99741243687622;-3.11771614754999;1.34978131827673]
Robot Arm 1 togoal: 15.3670166418251
Mobile Platform 2 Pose: [5.71363388838936;-2.62944739017008;-7.13592525646068]
Robot Arm 2 end: [-2.66644005797546;-7.14366289830504;1.43925001414197]
Robot Arm 1 togoal: 7.87060646468707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.523021e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-10.5509675768361;1.769200462159;-3.20152592189412]
Robot Arm 1 end: [2.14205139339954;-3.15327430003534;1.31700669086356]
Robot Arm 1 togoal: 15.3231065514182
Mobile Platform 2 Pose: [6.18973543388135;-2.87414417884354;-7.18715198465067]
Robot Arm 2 end: [-2.81051671117239;-7.13986992920754;1.44902917478681]
Robot Arm 1 togoal: 7.73784167464446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.333877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-10.6511118561163;1.82520272134597;-3.20655439853433]
Robot Arm 1 end: [2.28259213934711;-3.21084978084553;1.26812754545367]
Robot Arm 1 togoal: 15.3019240782982
Mobile Platform 2 Pose: [6.32757957745735;-2.97604717910169;-7.21683493492218]
Robot Arm 2 end: [-3.05902690470087;-7.14023401312656;1.45744960630449]
Robot Arm 1 togoal: 7.50729030089309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-10.7446478158965;1.87788960165404;-3.21057679756935]
Robot Arm 1 end: [2.42005125710829;-3.28281325696547;1.19576149944829]
Robot Arm 1 togoal: 15.2968797813974
Mobile Platform 2 Pose: [6.48542821758712;-3.10431350651216;-7.24274742786582]
Robot Arm 2 end: [-3.13366779882694;-6.99506020700684;1.41451010172009]
Robot Arm 1 togoal: 7.49583358638754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-10.8381684516929;1.9315010284083;-3.21401979556992]
Robot Arm 1 end: [2.55307006305395;-3.36562964740982;1.09095783172094]
Robot Arm 1 togoal: 15.3062653062072
Mobile Platform 2 Pose: [6.46619941615987;-3.08639830336445;-7.24613351309341]
Robot Arm 2 end: [-3.22626369147695;-7.04962166299121;1.43155237577489]
Robot Arm 1 togoal: 7.38892663289965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-10.9392397781643;1.99128419460166;-3.21719715981835]
Robot Arm 1 end: [2.66979788111836;-3.45343292098936;0.959145495860032]
Robot Arm 1 togoal: 15.3310926207834
Mobile Platform 2 Pose: [6.45256568287053;-3.06489677307408;-7.25381926944532]
Robot Arm 2 end: [-3.33803489635405;-7.10249687179162;1.42252009089497]
Robot Arm 1 togoal: 7.26546637957063

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-11.0604216134755;2.06712834410662;-3.22032773551766]
Robot Arm 1 end: [2.77190871363126;-3.5370167550199;0.809170114159495]
Robot Arm 1 togoal: 15.3623835408489
Mobile Platform 2 Pose: [6.4415498159864;-3.02711226239831;-7.27239167361035]
Robot Arm 2 end: [-3.35458108896769;-7.10468292344038;1.41505848141587]
Robot Arm 1 togoal: 7.2495294386797

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-11.1958256828418;2.15972400201502;-3.22474436997959]
Robot Arm 1 end: [2.83385370426308;-3.6425505599985;0.654797037081563]
Robot Arm 1 togoal: 15.4344791323513
Mobile Platform 2 Pose: [6.42683632288835;-3.00771889436651;-7.28294945037821]
Robot Arm 2 end: [-3.40351612993028;-7.11793931303063;1.38846477349089]
Robot Arm 1 togoal: 7.19982213931221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.284866e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-11.3268489878306;2.26721332003691;-3.23304017921141]
Robot Arm 1 end: [2.8430394459003;-3.75901884658189;0.511325108486109]
Robot Arm 1 togoal: 15.5419473483888
Mobile Platform 2 Pose: [6.46245565601345;-3.03511414616008;-7.27544307198831]
Robot Arm 2 end: [-3.51898192729771;-7.16040872890172;1.33844846696258]
Robot Arm 1 togoal: 7.07814939852258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.754049e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.613774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-11.4106906220104;2.36779257867674;-3.2473826287072]
Robot Arm 1 end: [2.88211613468644;-3.7617280884415;0.386411271335326]
Robot Arm 1 togoal: 15.5350160052542
Mobile Platform 2 Pose: [6.65865046499491;-3.1902393313087;-7.21378662518614]
Robot Arm 2 end: [-3.71670271589673;-7.34194148259319;1.26888659924422]
Robot Arm 1 togoal: 6.82705361112823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.244930e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.393630e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-11.4621457597902;2.456421312077;-3.25861627343247]
Robot Arm 1 end: [2.91516890755801;-3.79080669276483;0.34097266434527]
Robot Arm 1 togoal: 15.5490721703429
Mobile Platform 2 Pose: [6.12738499467132;-2.98128309740197;-7.07269933941099]
Robot Arm 2 end: [-3.68507103974484;-7.10759093276602;0.905418639971491]
Robot Arm 1 togoal: 6.97304082851242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.133343e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.743567e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-11.5451416161411;2.57856796307387;-3.2736583995526]
Robot Arm 1 end: [3.04332607824623;-3.65195393101703;0.179311462527165]
Robot Arm 1 togoal: 15.3808740427781
Mobile Platform 2 Pose: [6.31708642421351;-3.08256594315075;-7.08481708765638]
Robot Arm 2 end: [-3.75402407758215;-7.04481137211548;1.04645847766118]
Robot Arm 1 togoal: 6.92607715133359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.992907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.737371e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-11.6095616306348;2.65927356414151;-3.2805977214055]
Robot Arm 1 end: [3.16998783644677;-3.51597090551104;0.1539811968592]
Robot Arm 1 togoal: 15.2052384420977
Mobile Platform 2 Pose: [6.50798553948556;-3.20129658004196;-7.08512804379737]
Robot Arm 2 end: [-3.82765566538454;-6.97650190294875;1.15029452905705]
Robot Arm 1 togoal: 6.8830805511863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.017882e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.979044e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-11.6617723324529;2.70721847772714;-3.28179879092881]
Robot Arm 1 end: [3.15031570557476;-3.67369135280047;0.16946506938526]
Robot Arm 1 togoal: 15.3530015571424
Mobile Platform 2 Pose: [6.54730330770935;-3.27359380727406;-7.0234474341342]
Robot Arm 2 end: [-3.93537263737446;-6.82993752104698;1.09346305567407]
Robot Arm 1 togoal: 6.85651434817544

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.614843e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-11.7765685154284;2.80163698832201;-3.29457703377641]
Robot Arm 1 end: [3.14657457593386;-3.82049866074094;0.206983381653127]
Robot Arm 1 togoal: 15.4823209806563
Mobile Platform 2 Pose: [6.5362025136625;-3.31125704148955;-6.93117513524942]
Robot Arm 2 end: [-4.03856180061378;-6.83035141040259;0.985130557134873]
Robot Arm 1 togoal: 6.77292947301495

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009834e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-11.8802527540309;2.86048494826144;-3.30494597053435]
Robot Arm 1 end: [3.09073445382025;-3.89078221479572;0.190462661926535]
Robot Arm 1 togoal: 15.5711948974547
Mobile Platform 2 Pose: [6.5875782592162;-3.41195482084715;-6.79494967212038]
Robot Arm 2 end: [-4.09458881419105;-6.74818420358647;1.03496511840355]
Robot Arm 1 togoal: 6.75902486705076

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.728802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-11.9742297328145;2.91041156393427;-3.31416345501362]
Robot Arm 1 end: [3.03706195205373;-3.94147279553979;0.183222948490812]
Robot Arm 1 togoal: 15.6408700975316
Mobile Platform 2 Pose: [6.36995406937319;-3.32604562905913;-6.69419679874648]
Robot Arm 2 end: [-4.04908097242011;-6.59760865572506;0.875099910229443]
Robot Arm 1 togoal: 6.8852662300288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.391286e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-12.0684398493655;2.95921218182583;-3.32348627498824]
Robot Arm 1 end: [2.98924202266444;-3.97914541275547;0.181125100012399]
Robot Arm 1 togoal: 15.6959389217515
Mobile Platform 2 Pose: [6.09510515056537;-3.19127767671816;-6.62788396402828]
Robot Arm 2 end: [-3.92836034954161;-6.54752562999335;0.726716801554248]
Robot Arm 1 togoal: 7.02959978376468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.340043e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-12.1698868337406;3.01131450354218;-3.33397773197592]
Robot Arm 1 end: [2.95024915284433;-4.0074162073787;0.182748407392069]
Robot Arm 1 togoal: 15.738412027229
Mobile Platform 2 Pose: [5.82111540409119;-3.05637304831744;-6.56871381504878]
Robot Arm 2 end: [-3.77941320385175;-6.49009950503757;0.569661669640727]
Robot Arm 1 togoal: 7.20556355823486

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.341773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-12.2847398727555;3.07017902052236;-3.34671524458685]
Robot Arm 1 end: [2.92096800817368;-4.02798817642378;0.186648588791574]
Robot Arm 1 togoal: 15.7695161595966
Mobile Platform 2 Pose: [5.56892975607601;-2.93120044473894;-6.51841432541723]
Robot Arm 2 end: [-3.65163103366356;-6.45273785548643;0.453875500201798]
Robot Arm 1 togoal: 7.35007564265241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.222847e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-12.4210731946811;3.13991348936078;-3.36327708722779]
Robot Arm 1 end: [2.90174271499512;-4.04026166064591;0.190383616415274]
Robot Arm 1 togoal: 15.7887537172442
Mobile Platform 2 Pose: [5.19958614355556;-2.82973661547926;-6.37698012601294]
Robot Arm 2 end: [-3.4491790857805;-6.14019322036236;0.253649593044718]
Robot Arm 1 togoal: 7.70827737437633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.534627e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-12.5920247637549;3.22637113748167;-3.38656861965968]
Robot Arm 1 end: [2.88946318493406;-4.05107721289432;0.208651786579455]
Robot Arm 1 togoal: 15.8023657239792
Mobile Platform 2 Pose: [4.82033012888438;-2.69728364928698;-6.2697036251088]
Robot Arm 2 end: [-3.13297334449704;-5.81215695256223;0.15558543344694]
Robot Arm 1 togoal: 8.15833570134965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.190029e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-12.82035126754;3.33803423701419;-3.42201795911851]
Robot Arm 1 end: [2.87661550482836;-4.05446457242912;0.254817082491642]
Robot Arm 1 togoal: 15.8073970110296
Mobile Platform 2 Pose: [5.19385700807073;-2.94312522235526;-6.18905201475423]
Robot Arm 2 end: [-3.5420090530587;-6.1252188779184;0.186417796102655]
Robot Arm 1 togoal: 7.64857408108043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.882903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-13.1536842325381;3.48273783429273;-3.47689655840457]
Robot Arm 1 end: [2.89841677859477;-4.05942760063449;0.34595456080507]
Robot Arm 1 togoal: 15.7949587049856
Mobile Platform 2 Pose: [4.81157621866828;-2.7528625188139;-6.15123361462286]
Robot Arm 2 end: [-3.19558256591099;-5.97220950978195;0.0290267840766289]
Robot Arm 1 togoal: 8.04668732631374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.358319e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-13.4549452551393;3.55841370614985;-3.48075803553168]
Robot Arm 1 end: [2.91400337282715;-3.92510279875014;0.253388691934863]
Robot Arm 1 togoal: 15.6756714237189
Mobile Platform 2 Pose: [4.381675799581;-2.54360784548698;-6.09577454162228]
Robot Arm 2 end: [-2.73082709173883;-5.81016344989624;-0.0837229300902149]
Robot Arm 1 togoal: 8.5422913067362

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.093944e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.603271e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-13.6804964716365;3.62846839499712;-3.48878768686473]
Robot Arm 1 end: [2.95937540734886;-3.85863137978574;0.221717677707798]
Robot Arm 1 togoal: 15.5987240801434
Mobile Platform 2 Pose: [3.72918166951469;-2.26052493623866;-5.92264448862182]
Robot Arm 2 end: [-2.09795279324244;-5.7189660804872;-0.139511545954792]
Robot Arm 1 togoal: 9.1393052291787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.251523e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.248998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-14.0016482087475;3.70694809520104;-3.50720947399369]
Robot Arm 1 end: [3.04907642743516;-3.88032152322466;0.221059035841476]
Robot Arm 1 togoal: 15.577821112403
Mobile Platform 2 Pose: [3.3162371153689;-2.08927758814629;-5.6842531912953]
Robot Arm 2 end: [-1.68566936933434;-5.60049206829063;-0.0854258610151981]
Robot Arm 1 togoal: 9.54276521371753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.852949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.727511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-14.3789243058422;3.76606491131636;-3.55249010696482]
Robot Arm 1 end: [3.17973322675684;-3.9902679537042;0.2073492461971]
Robot Arm 1 togoal: 15.6195171044951
Mobile Platform 2 Pose: [3.10533697939198;-2.00519320553761;-5.48735727087547]
Robot Arm 2 end: [-1.61189582959656;-5.58262190210098;-0.0836242071523883]
Robot Arm 1 togoal: 9.61500596357158

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.913174e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.941064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-14.473682020055;3.6493274096794;-3.71370505215728]
Robot Arm 1 end: [3.03054435049862;-4.09598608643614;0.232412024495054]
Robot Arm 1 togoal: 15.7775344959212
Mobile Platform 2 Pose: [2.61652008228251;-1.76244182255632;-5.25831236154153]
Robot Arm 2 end: [-1.29839798357601;-5.54155597070016;-0.0152172230084789]
Robot Arm 1 togoal: 9.89725033385037

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.333156e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.919956e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-14.2042840309969;3.42186657304189;-3.88262893893226]
Robot Arm 1 end: [2.77538592449937;-4.19424368423703;0.232005101703208]
Robot Arm 1 togoal: 15.9791461021939
Mobile Platform 2 Pose: [2.0300736061828;-1.4491367636039;-4.89938262121389]
Robot Arm 2 end: [-1.00252780596374;-5.37922551870688;0.0893022514750797]
Robot Arm 1 togoal: 10.2154696971092

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.004642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.766299e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-13.8334878620006;3.10150502891641;-3.99912351509219]
Robot Arm 1 end: [2.46768337515736;-4.13395221218765;0.155475369415215]
Robot Arm 1 togoal: 16.0738625224312
Mobile Platform 2 Pose: [1.22584690060918;-1.15508681706505;-4.84804046258917]
Robot Arm 2 end: [-0.613059631837166;-5.26236883005819;0.142245061449625]
Robot Arm 1 togoal: 10.6047519423335

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.569264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-13.344222294689;2.76907847855086;-4.05208959906434]
Robot Arm 1 end: [2.16435373225562;-4.1254938672152;0.108275247238118]
Robot Arm 1 togoal: 16.2148919408369
Mobile Platform 2 Pose: [1.72496940197502;-1.34294637891981;-4.87742948901699]
Robot Arm 2 end: [-0.720076957709745;-5.2679336444742;0.214147319814146]
Robot Arm 1 togoal: 10.4984421507021

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.281939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-12.9480928401914;2.5003271446507;-4.06485895620128]
Robot Arm 1 end: [1.85032336752787;-4.00506155859696;0.174391291828297]
Robot Arm 1 togoal: 16.2595305432499
Mobile Platform 2 Pose: [1.32581382121589;-1.16062723967942;-4.92588308529177]
Robot Arm 2 end: [-0.663210124218953;-5.47538912307213;0.242088261874896]
Robot Arm 1 togoal: 10.4538683752004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.725587e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-12.5812665959922;2.24137917009166;-4.04458798521462]
Robot Arm 1 end: [1.58241016983656;-3.98188910807047;0.18599597909361]
Robot Arm 1 togoal: 16.3747145720189
Mobile Platform 2 Pose: [0.814466581674653;-0.932357571914139;-4.96978873965009]
Robot Arm 2 end: [-0.601608413833995;-5.62547444438032;0.264265568035122]
Robot Arm 1 togoal: 10.4424910666072

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.326460e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-12.2039145609953;1.98291975111572;-4.04398238511423]
Robot Arm 1 end: [1.30391065387182;-3.9794882154997;0.229825564604687]
Robot Arm 1 togoal: 16.5140907138649
Mobile Platform 2 Pose: [0.23507660131184;-0.699498878452281;-5.06076582124306]
Robot Arm 2 end: [-0.446742254268638;-5.70686103574968;0.249659776342743]
Robot Arm 1 togoal: 10.5504540986172

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.696073e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-11.8365309600936;1.73318368994779;-4.05546714797694]
Robot Arm 1 end: [1.04586719349107;-3.97533974741326;0.247694811713158]
Robot Arm 1 togoal: 16.6465588478444
Mobile Platform 2 Pose: [-0.116806132424961;-0.52599129377317;-5.12202593102204]
Robot Arm 2 end: [-0.460321129044016;-5.80428284456875;0.271418297365553]
Robot Arm 1 togoal: 10.4962359844328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.049510e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-11.4603500829569;1.48403449038095;-4.07608391821149]
Robot Arm 1 end: [0.781174176241323;-4.05412163355768;0.287916922320392]
Robot Arm 1 togoal: 16.8530584267691
Mobile Platform 2 Pose: [-0.198731661171375;-0.434688598904499;-5.35475714489272]
Robot Arm 2 end: [-0.44832043147899;-5.99026182397896;0.226754887481042]
Robot Arm 1 togoal: 10.4397151891812
Step 213:
Mobile Platform 1 Pose: [-11.0512893505521;1.24171679086887;-4.13758296297239]
Robot Arm 1 end: [0.562604093102507;-4.21016979478643;0.258623751096304]
Robot Arm 1 togoal: 17.1051735125366
Mobile Platform 2 Pose: [0.229136862362512;-0.564045621630746;-5.41354750320749]
Robot Arm 2 end: [-0.504385464771841;-6.06505098284148;0.236501618865823]
Robot Arm 1 togoal: 10.3585933050974
Step 214:
Mobile Platform 1 Pose: [-10.7318206899099;1.15805420035586;-4.26318795951125]
Robot Arm 1 end: [0.47507579306;-4.28293509851638;0.297593793549321]
Robot Arm 1 togoal: 17.2111341556088
Mobile Platform 2 Pose: [0.449020732879336;-0.643397092383512;-5.41299924086371]
Robot Arm 2 end: [-0.443708145893493;-6.03570846196455;0.20000526165797]
Robot Arm 1 togoal: 10.4299256228474
Step 215:
Mobile Platform 1 Pose: [-10.4236423537189;1.08188578020768;-4.43168037523489]
Robot Arm 1 end: [0.405328772236492;-4.50531462207906;0.280674150412959]
Robot Arm 1 togoal: 17.435603702411
Mobile Platform 2 Pose: [0.609031266517147;-0.706445747734485;-5.40702231472645]
Robot Arm 2 end: [-0.391059727782811;-6.00233431587831;0.187282746373955]
Robot Arm 1 togoal: 10.4924670973828
Step 216:
Mobile Platform 1 Pose: [-10.4852533452788;1.26264191622135;-4.51920857695062]
Robot Arm 1 end: [0.59095079366917;-4.548726850311;0.274454744338177]
Robot Arm 1 togoal: 17.3709393818103
Mobile Platform 2 Pose: [0.730107472824587;-0.758549208500349;-5.3994785364991]
Robot Arm 2 end: [-0.330735208896397;-5.95413269980675;0.186996260286016]
Robot Arm 1 togoal: 10.5661388306618
Step 217:
Mobile Platform 1 Pose: [-10.6288841107237;1.44579712278999;-4.56655865053836]
Robot Arm 1 end: [0.748245339170886;-4.60933792221779;0.337019340742054]
Robot Arm 1 togoal: 17.3329030756888
Mobile Platform 2 Pose: [0.837581528989779;-0.807091962838882;-5.39136698627703]
Robot Arm 2 end: [-0.266987866502733;-5.88300828395454;0.197813614514289]
Robot Arm 1 togoal: 10.6504445067578
Step 218:
Mobile Platform 1 Pose: [-10.7479941857493;1.62813500632452;-4.60567349930237]
Robot Arm 1 end: [0.885072424359765;-4.61480719918032;0.454851707726629]
Robot Arm 1 togoal: 17.2572062153611
Mobile Platform 2 Pose: [0.973938787349551;-0.863488962602624;-5.38140263453284]
Robot Arm 2 end: [-0.23678013966717;-5.81271371634042;0.219377365930506]
Robot Arm 1 togoal: 10.7027122704501
Step 219:
Mobile Platform 1 Pose: [-10.3172928921723;1.39775553673113;-4.70275748391565]
Robot Arm 1 end: [0.62225393875485;-4.73750190525135;0.528767631033575]
Robot Arm 1 togoal: 17.4963027143025
Mobile Platform 2 Pose: [1.16032783876668;-0.911642630149633;-5.37045372024108]
Robot Arm 2 end: [-0.24408899108268;-5.76756466723147;0.243728358628969]
Robot Arm 1 togoal: 10.7108697735446
Step 220:
Mobile Platform 1 Pose: [-10.0370428765023;1.15108155728656;-4.74340164542733]
Robot Arm 1 end: [0.392046638097404;-4.76759488449663;0.449037286580109]
Robot Arm 1 togoal: 17.65060142137
Mobile Platform 2 Pose: [1.32730622507283;-0.905848984708399;-5.36494329869979]
Robot Arm 2 end: [-0.22425315652604;-5.71474447398978;0.239831517956159]
Robot Arm 1 togoal: 10.7503504105808
Step 221:
Mobile Platform 1 Pose: [-9.7072674081798;0.905812733268748;-4.79180829750054]
Robot Arm 1 end: [0.119961806369694;-4.84747735169658;0.495008685446515]
Robot Arm 1 togoal: 17.8638012918381
Mobile Platform 2 Pose: [1.50399392599463;-0.864936530163518;-5.36126499243631]
Robot Arm 2 end: [-0.190814148485022;-5.65101157336274;0.215972187934107]
Robot Arm 1 togoal: 10.8091130531072
Step 222:
Mobile Platform 1 Pose: [-9.50457201646111;0.683783592621245;-4.8293289085476]
Robot Arm 1 end: [-0.0782272003098201;-4.86786181564434;0.407017721535759]
Robot Arm 1 togoal: 17.9962478029975
Mobile Platform 2 Pose: [1.66021213271443;-0.802502217363471;-5.35162828193758]
Robot Arm 2 end: [-0.0389839063240386;-5.47086748941387;0.282598842244003]
Robot Arm 1 togoal: 11.0121987061827
Step 223:
Mobile Platform 1 Pose: [-9.21000442703569;0.444387766211794;-4.87110784667877]
Robot Arm 1 end: [-0.318313555133515;-4.94168363025848;0.388704750757009]
Robot Arm 1 togoal: 18.1935042490512
Mobile Platform 2 Pose: [2.09833644954856;-0.918076936498869;-5.30590639099983]
Robot Arm 2 end: [-0.132974206857126;-5.50684814695996;0.377891276867192]
Robot Arm 1 togoal: 10.9019864763228
Step 224:
Mobile Platform 1 Pose: [-8.94695473029129;0.211620390788304;-4.91967823311134]
Robot Arm 1 end: [-0.513003032657425;-4.96955934673173;0.311614711836105]
Robot Arm 1 togoal: 18.3323089810099
Mobile Platform 2 Pose: [2.49376330838057;-0.965741562446522;-5.28734357326062]
Robot Arm 2 end: [-0.208865554061203;-5.58203156752707;0.419542236282553]
Robot Arm 1 togoal: 10.7980538993853
Step 225:
Mobile Platform 1 Pose: [-8.57431837404718;-0.0222182386301207;-4.9824289401963]
Robot Arm 1 end: [-0.757625728385867;-5.06877130090806;0.327698802439578]
Robot Arm 1 togoal: 18.5531223134867
Mobile Platform 2 Pose: [2.62783698041413;-0.867426839915883;-5.34430959682211]
Robot Arm 2 end: [-0.104554292157267;-5.63520653549929;0.522435431400333]
Robot Arm 1 togoal: 10.8613258372205
Step 226:
Mobile Platform 1 Pose: [-8.24133523089953;-0.247755136533911;-5.04367855998878]
Robot Arm 1 end: [-0.934446129785826;-5.1054849481105;0.257796833596688]
Robot Arm 1 togoal: 18.6904646791457
Mobile Platform 2 Pose: [2.7580985502156;-0.777168524531964;-5.39592161939639]
Robot Arm 2 end: [-0.00643395981357686;-5.66868717409759;0.610141204043926]
Robot Arm 1 togoal: 10.9298351653428
Step 227:
Mobile Platform 1 Pose: [-7.88896861883786;-0.375955737495278;-5.13694415388189]
Robot Arm 1 end: [-1.09890982254355;-5.22973841302127;0.331145001306699]
Robot Arm 1 togoal: 18.8824385685728
Mobile Platform 2 Pose: [2.9470941473341;-0.712234312530567;-5.44270691718401]
Robot Arm 2 end: [-0.0077590495433032;-5.81118295129501;0.741515903544831]
Robot Arm 1 togoal: 10.8627189476729
Step 228:
Mobile Platform 1 Pose: [-7.50058675337843;-0.58888372285528;-5.21376305438843]
Robot Arm 1 end: [-1.2621128573112;-5.28052227519895;0.265988916696194]
Robot Arm 1 togoal: 19.0237903645115
Mobile Platform 2 Pose: [3.11008692311935;-0.704507713374783;-5.49296842375796]
Robot Arm 2 end: [-0.0789194974935794;-5.94598395677507;0.826462620444795]
Robot Arm 1 togoal: 10.7398913675855
Step 229:
Mobile Platform 1 Pose: [-7.5312677844045;-0.44214560716297;-5.284415286946]
Robot Arm 1 end: [-1.14140518475777;-5.35500557943478;0.328904877032394]
Robot Arm 1 togoal: 19.0086347910002
Mobile Platform 2 Pose: [3.26863028039403;-0.742550267053256;-5.53045652968584]
Robot Arm 2 end: [-0.246051330224662;-6.03032337475477;0.994675945828408]
Robot Arm 1 togoal: 10.5439491734184
Step 230:
Mobile Platform 1 Pose: [-7.51285406501197;-0.302083695310901;-5.35757578373794]
Robot Arm 1 end: [-1.01095349018139;-5.39971295373462;0.353551842561988]
Robot Arm 1 togoal: 18.9672135808259
Mobile Platform 2 Pose: [3.4295144784862;-0.8007493418532;-5.55553139283018]
Robot Arm 2 end: [-0.456404036755768;-6.08405893595867;1.10084967980958]
Robot Arm 1 togoal: 10.3243084329292
Step 231:
Mobile Platform 1 Pose: [-7.55832740132644;-0.155095519383975;-5.41901033005498]
Robot Arm 1 end: [-0.905906035935883;-5.44560107332912;0.41762131415382]
Robot Arm 1 togoal: 18.9399795011892
Mobile Platform 2 Pose: [3.54114520661895;-0.84956060182031;-5.57167801601277]
Robot Arm 2 end: [-0.654726232020606;-6.07239026472767;1.20098397421201]
Robot Arm 1 togoal: 10.1421235590901
Step 232:
Mobile Platform 1 Pose: [-7.26313648275594;-0.384137127611117;-5.43064833951442]
Robot Arm 1 end: [-1.11399412008328;-5.46226478373229;0.337911678636216]
Robot Arm 1 togoal: 19.0788415665009
Mobile Platform 2 Pose: [3.66828796518616;-0.910895420434133;-5.58766210642819]
Robot Arm 2 end: [-0.847371206104154;-6.01474523737736;1.27874089015979]
Robot Arm 1 togoal: 9.98556752709421
Step 233:
Mobile Platform 1 Pose: [-6.97356862682935;-0.603657786369952;-5.44791312555691]
Robot Arm 1 end: [-1.37806755973094;-5.49482228467466;0.411198574256588]
Robot Arm 1 togoal: 19.2556900222255
Mobile Platform 2 Pose: [3.85195490999355;-1.00574467514564;-5.60786734712803]
Robot Arm 2 end: [-1.04007314664513;-5.92303716474645;1.33225406041406]
Robot Arm 1 togoal: 9.84568637562758
Step 234:
Mobile Platform 1 Pose: [-6.79766859572432;-0.753766869612151;-5.45564609917805]
Robot Arm 1 end: [-1.51927113863917;-5.5019143885089;0.373203203639872]
Robot Arm 1 togoal: 19.3473614418059
Mobile Platform 2 Pose: [4.14712607260887;-1.16934524627085;-5.63626249821316]
Robot Arm 2 end: [-1.25951204680021;-5.81094549624012;1.37071325346199]
Robot Arm 1 togoal: 9.693652222031
Step 235:
Mobile Platform 1 Pose: [-6.62067810970188;-0.904226734597957;-5.46335538583164]
Robot Arm 1 end: [-1.65773800597219;-5.51556247521824;0.333367316702803]
Robot Arm 1 togoal: 19.4434051989029
Mobile Platform 2 Pose: [4.58168197858256;-1.41583574403906;-5.67800497669709]
Robot Arm 2 end: [-1.4681435621618;-5.6493069909321;1.40128110500321]
Robot Arm 1 togoal: 9.57786179420296
Step 236:
Mobile Platform 1 Pose: [-6.44130987371662;-1.05647465243712;-5.4719487312575]
Robot Arm 1 end: [-1.79496410821979;-5.52328106251674;0.291392791555271]
Robot Arm 1 togoal: 19.5347220903483
Mobile Platform 2 Pose: [3.968833667631;-1.18199758311747;-5.58957582598972]
Robot Arm 2 end: [-1.26213450059004;-5.66981252361565;1.41017373187177]
Robot Arm 1 togoal: 9.75259450239381
Step 237:
Mobile Platform 1 Pose: [-6.25774124059552;-1.21071471438051;-5.48082930453576]
Robot Arm 1 end: [-1.92962585645342;-5.53863607373527;0.246769031488412]
Robot Arm 1 togoal: 19.6313231512311
Mobile Platform 2 Pose: [3.64276865388126;-1.20127831478986;-5.50050101368672]
Robot Arm 2 end: [-1.3423303547562;-5.47072199165627;1.41534410659088]
Robot Arm 1 togoal: 9.77142600293602
Step 238:
Mobile Platform 1 Pose: [-6.06768765998328;-1.36901587789124;-5.49136936826297]
Robot Arm 1 end: [-2.06362980956425;-5.54548545011085;0.199257280317923]
Robot Arm 1 togoal: 19.7215690521532
Mobile Platform 2 Pose: [3.27734988072613;-1.18014884760612;-5.41027442733082]
Robot Arm 2 end: [-1.42198205547872;-5.2863669374144;1.38548431089808]
Robot Arm 1 togoal: 9.7887227464681
Step 239:
Mobile Platform 1 Pose: [-5.86842212800058;-1.53107301631679;-5.50267318756793]
Robot Arm 1 end: [-2.20126862649109;-5.56797041294728;0.157128756740181]
Robot Arm 1 togoal: 19.826568125684
Mobile Platform 2 Pose: [3.01122430243124;-1.22219807103492;-5.32668867718928]
Robot Arm 2 end: [-1.49731379706791;-5.11816569728684;1.34992143234181]
Robot Arm 1 togoal: 9.80598014993864
Step 240:
Mobile Platform 1 Pose: [-5.67158908838799;-1.69046041411286;-5.51611960839201]
Robot Arm 1 end: [-2.38048032437755;-5.57783958124765;0.187390846505378]
Robot Arm 1 togoal: 19.9430161323625
Mobile Platform 2 Pose: [2.56952405461226;-1.11822600368;-5.23245977993656]
Robot Arm 2 end: [-1.55823332327877;-4.96716057306397;1.24962367098967]
Robot Arm 1 togoal: 9.83191448464997
Step 241:
Mobile Platform 1 Pose: [-5.48566191852341;-1.84417738804255;-5.52733092348451]
Robot Arm 1 end: [-2.50176065312413;-5.6005838155766;0.138175315103899]
Robot Arm 1 togoal: 20.0395718248667
Mobile Platform 2 Pose: [2.31472154109643;-1.11262764804191;-5.14327149674814]
Robot Arm 2 end: [-1.60531578004409;-4.81186316249587;1.16684367084969]
Robot Arm 1 togoal: 9.87486271818101
Step 242:
Mobile Platform 1 Pose: [-5.29032615677676;-2.00129853230159;-5.54271824810415]
Robot Arm 1 end: [-2.69175437925787;-5.61218619962501;0.187863367980833]
Robot Arm 1 togoal: 20.1642813022137
Mobile Platform 2 Pose: [1.98229894859102;-1.09148786550138;-5.03405398114524]
Robot Arm 2 end: [-1.60470468674408;-4.68437362931295;1.11553184545754]
Robot Arm 1 togoal: 9.9449118414327
Step 243:
Mobile Platform 1 Pose: [-5.11393685966558;-2.1490716453665;-5.55434851450966]
Robot Arm 1 end: [-2.80186002372075;-5.63610537860324;0.133487763178215]
Robot Arm 1 togoal: 20.2558782007854
Mobile Platform 2 Pose: [2.17527969042977;-1.3031603120203;-4.96880037280044]
Robot Arm 2 end: [-1.75537826602942;-4.42520383544726;1.01739017241047]
Robot Arm 1 togoal: 9.96522767783139
Step 244:
Mobile Platform 1 Pose: [-4.91861763438934;-2.3044209507716;-5.57216176125703]
Robot Arm 1 end: [-2.99326324310631;-5.64930663708453;0.188910657264475]
Robot Arm 1 togoal: 20.3838207886349
Mobile Platform 2 Pose: [1.96907800378517;-1.35124429791625;-4.81936201327163]
Robot Arm 2 end: [-1.83770150165223;-4.41883732403989;1.12442910374396]
Robot Arm 1 togoal: 9.89594674942311
Step 245:
Mobile Platform 1 Pose: [-4.74089179204131;-2.45218877394679;-5.58581790888823]
Robot Arm 1 end: [-3.10323677042504;-5.67684394975538;0.134993080249017]
Robot Arm 1 togoal: 20.4787515554147
Mobile Platform 2 Pose: [2.01874658787669;-1.52671848145237;-4.70022876486335]
Robot Arm 2 end: [-1.91610441887914;-4.12496377005491;1.0112164220294]
Robot Arm 1 togoal: 10.0062629270873
Step 246:
Mobile Platform 1 Pose: [-4.54155498315361;-2.60713494859729;-5.60707955309199]
Robot Arm 1 end: [-3.2928839008937;-5.69233190110001;0.190643347093965]
Robot Arm 1 togoal: 20.6087333846807
Mobile Platform 2 Pose: [1.65523750465548;-1.42487357843431;-4.5934136371288]
Robot Arm 2 end: [-1.93343347163947;-4.15892745730606;1.0614104097776]
Robot Arm 1 togoal: 9.96989702037691
Step 247:
Mobile Platform 1 Pose: [-4.35085041635119;-2.76048075963098;-5.62490813002701]
Robot Arm 1 end: [-3.40890097939926;-5.72628141140515;0.137305548036603]
Robot Arm 1 togoal: 20.7130191628945
Mobile Platform 2 Pose: [1.67678042898056;-1.5449364241415;-4.49445070341364]
Robot Arm 2 end: [-1.94789037906201;-3.88877215996238;0.947389765507887]
Robot Arm 1 togoal: 10.1248508019411
Step 248:
Mobile Platform 1 Pose: [-4.13852246598447;-2.91738190276072;-5.651893322639]
Robot Arm 1 end: [-3.59833810904055;-5.74634139539303;0.193604405538613]
Robot Arm 1 togoal: 20.8476388526914
Mobile Platform 2 Pose: [1.32632723556346;-1.44031230453046;-4.3972208165463]
Robot Arm 2 end: [-1.98072202131358;-3.92209827265437;0.973985904656418]
Robot Arm 1 togoal: 10.0771490575073
Step 249:
Mobile Platform 1 Pose: [-3.91459016551713;-3.08406572526019;-5.67839421474123]
Robot Arm 1 end: [-3.72885024295648;-5.79241144836018;0.141117193526669]
Robot Arm 1 togoal: 20.9710673287609
Mobile Platform 2 Pose: [1.31024021497052;-1.54272937389451;-4.29386290574425]
Robot Arm 2 end: [-1.9833839262685;-3.67976062028517;0.879351205950877]
Robot Arm 1 togoal: 10.2285518166265
Step 250:
Mobile Platform 1 Pose: [-3.67881170992914;-3.24214200045323;-5.71448833449368]
Robot Arm 1 end: [-3.91603640694557;-5.81992893381894;0.198955682532447]
Robot Arm 1 togoal: 21.1109929166741
Mobile Platform 2 Pose: [0.977932684719032;-1.45612905508507;-4.19218453011106]
Robot Arm 2 end: [-2.03375643040229;-3.71757260742575;0.90893928146491]
Robot Arm 1 togoal: 10.1638845367457
Step 251:
Mobile Platform 1 Pose: [-3.3879314093571;-3.42671443318368;-5.75975555980222]
Robot Arm 1 end: [-4.07143159508616;-5.89028518241092;0.155294603683489]
Robot Arm 1 togoal: 21.2690268123171
Mobile Platform 2 Pose: [0.98770312466517;-1.56667774823658;-4.09841531163395]
Robot Arm 2 end: [-2.01679262083309;-3.46352483066948;0.809014753099498]
Robot Arm 1 togoal: 10.342341644418
Step 252:
Mobile Platform 1 Pose: [-3.12839260895428;-3.57546179768634;-5.8065119417292]
Robot Arm 1 end: [-4.23973158101971;-5.92414350142917;0.207978107781831]
Robot Arm 1 togoal: 21.4026243307461
Mobile Platform 2 Pose: [0.660480527558799;-1.47564539586834;-4.00351352147288]
Robot Arm 2 end: [-2.07416089347419;-3.50272029254811;0.813004001412938]
Robot Arm 1 togoal: 10.2730145270712
Step 253:
Mobile Platform 1 Pose: [-2.77711019095036;-3.73301925288324;-5.88371737188272]
Robot Arm 1 end: [-4.40497002137697;-6.03172863620573;0.222960657032822]
Robot Arm 1 togoal: 21.5917667298811
Mobile Platform 2 Pose: [0.645839805041194;-1.57565941736194;-3.90618399910157]
Robot Arm 2 end: [-2.04970258028519;-3.2671661002926;0.726871367857784]
Robot Arm 1 togoal: 10.4483932576909
Step 254:
Mobile Platform 1 Pose: [-2.49905511723247;-3.87926391685571;-5.93718175674723]
Robot Arm 1 end: [-4.53211943483999;-6.06365761967779;0.218384219727803]
Robot Arm 1 togoal: 21.7007004325137
Mobile Platform 2 Pose: [0.326728198997688;-1.49255690558048;-3.80946062293174]
Robot Arm 2 end: [-2.11795536226981;-3.30743931471473;0.704115076633512]
Robot Arm 1 togoal: 10.3722850508179
Step 255:
Mobile Platform 1 Pose: [-2.32716163611611;-3.84677552616593;-6.02495193769026]
Robot Arm 1 end: [-4.53121246878866;-6.16527122034893;0.286520760616936]
Robot Arm 1 togoal: 21.7714507654632
Mobile Platform 2 Pose: [0.32532937596349;-1.59880016378295;-3.71480836810529]
Robot Arm 2 end: [-2.07599327351982;-3.06693959733469;0.626845688255316]
Robot Arm 1 togoal: 10.566774393424
Step 256:
Mobile Platform 1 Pose: [-2.02227113771478;-3.92958045044021;-6.11269014636536]
Robot Arm 1 end: [-4.56431952042123;-6.22850424984629;0.226169857173887]
Robot Arm 1 togoal: 21.8440000544537
Mobile Platform 2 Pose: [0.0118982726054905;-1.52186610058706;-3.61285993081924]
Robot Arm 2 end: [-2.14987040417808;-3.10599580298772;0.543805809910753]
Robot Arm 1 togoal: 10.4931757359136
Step 257:
Mobile Platform 1 Pose: [-2.17965922336042;-3.7817992465269;-6.11966268714422]
Robot Arm 1 end: [-4.44645237205613;-6.2333621162649;0.29226334567916]
Robot Arm 1 togoal: 21.7653813543788
Mobile Platform 2 Pose: [0.0465430491417261;-1.64076690340103;-3.5220323300536]
Robot Arm 2 end: [-2.11400317200341;-2.88137526255613;0.49988928579493]
Robot Arm 1 togoal: 10.6726955926975
Step 258:
Mobile Platform 1 Pose: [-2.31241808530167;-3.65509839444987;-6.12461109361986]
Robot Arm 1 end: [-4.36531428048051;-6.21764666791152;0.371564851031267]
Robot Arm 1 togoal: 21.695518407816
Mobile Platform 2 Pose: [-0.202997519761216;-1.6289191128427;-3.38274425057255]
Robot Arm 2 end: [-2.22740649762349;-2.90763983472329;0.341088172654192]
Robot Arm 1 togoal: 10.5880581027637
Step 259:
Mobile Platform 1 Pose: [-2.46795049035102;-3.41901980855541;-6.19240612251886]
Robot Arm 1 end: [-4.17197591175933;-6.26139550456377;0.443147942030481]
Robot Arm 1 togoal: 21.597213942039
Mobile Platform 2 Pose: [-0.0680611139673966;-1.77611593626128;-3.31654022261035]
Robot Arm 2 end: [-2.24055414027262;-2.69405350798562;0.4431527050775]
Robot Arm 1 togoal: 10.7120310392282
Step 260:
Mobile Platform 1 Pose: [-2.3317533605423;-3.52518641444418;-6.18982998592626]
Robot Arm 1 end: [-4.2828581786177;-6.26521438419675;0.426487806393712]
Robot Arm 1 togoal: 21.6738366159833
Mobile Platform 2 Pose: [-0.0916953775418609;-1.90374838406778;-3.15408346945302]
Robot Arm 2 end: [-2.29027658678862;-2.49050085333503;0.448770896476788]
Robot Arm 1 togoal: 10.8158258026109
Step 261:
Mobile Platform 1 Pose: [-2.22653948141763;-3.60730117305623;-6.1884830669634]
Robot Arm 1 end: [-4.37321706102206;-6.2616484940359;0.427102695196081]
Robot Arm 1 togoal: 21.7307890270992
Mobile Platform 2 Pose: [-0.342921323427355;-1.88436198916532;-3.00174140120996]
Robot Arm 2 end: [-2.31209258054427;-2.40184767223508;0.319204017764959]
Robot Arm 1 togoal: 10.8756679274865
Step 262:
Mobile Platform 1 Pose: [-2.13665787716866;-3.67597544414521;-6.18745463058206]
Robot Arm 1 end: [-4.45097087316541;-6.26150286243602;0.43639987478719]
Robot Arm 1 togoal: 21.781721510333
Mobile Platform 2 Pose: [-0.443700547178494;-1.96817412872016;-2.83958366821803]
Robot Arm 2 end: [-2.30837096550878;-2.22905145295523;0.234507239370353]
Robot Arm 1 togoal: 11.0092625615069
Step 263:
Mobile Platform 1 Pose: [-2.059375407744;-3.73406010794414;-6.18676291141399]
Robot Arm 1 end: [-4.5191412222884;-6.2621252975467;0.45398501288361]
Robot Arm 1 togoal: 21.8266053707074
Mobile Platform 2 Pose: [-0.640448266049445;-1.98708917619347;-2.67659643910678]
Robot Arm 2 end: [-2.36871457357276;-2.13392966996321;0.168122060638845]
Robot Arm 1 togoal: 11.0427287576711
Step 264:
Mobile Platform 1 Pose: [-1.98973279889832;-3.7854165504821;-6.18637537740913]
Robot Arm 1 end: [-4.58078396879417;-6.26366171951489;0.478275923221614]
Robot Arm 1 togoal: 21.867629665141
Mobile Platform 2 Pose: [-0.484225317800818;-2.17868475095829;-2.5510193271836]
Robot Arm 2 end: [-2.46827474464784;-1.88600331004582;0.303195201092582]
Robot Arm 1 togoal: 11.1376333273698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.219136e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-1.92366106564481;-3.83314274094395;-6.18632437553714]
Robot Arm 1 end: [-4.64208562638078;-6.19838254442499;0.508276280696798]
Robot Arm 1 togoal: 21.8587254403691
Mobile Platform 2 Pose: [-0.800897893767691;-2.06628155253185;-2.4651412912619]
Robot Arm 2 end: [-2.50153773734194;-1.91790484189552;0.251675694899327]
Robot Arm 1 togoal: 11.0976746501545
Step 266:
Mobile Platform 1 Pose: [-1.84823233601888;-3.88351292403838;-6.18514219959132]
Robot Arm 1 end: [-4.69711612168573;-6.26821659090162;0.546860015174206]
Robot Arm 1 togoal: 21.9455927705961
Mobile Platform 2 Pose: [-0.922635240042782;-2.09340711613115;-2.35853337833085]
Robot Arm 2 end: [-2.54675952749128;-1.88052237757239;0.173012758885161]
Robot Arm 1 togoal: 11.1037732327508
Step 267:
Mobile Platform 1 Pose: [-1.77784744674639;-3.93236130613398;-6.1862342201037]
Robot Arm 1 end: [-4.75217997690242;-6.27266873205909;0.587209332419152]
Robot Arm 1 togoal: 21.9840516453432
Mobile Platform 2 Pose: [-1.08907575022968;-2.15894179152662;-2.21246431854455]
Robot Arm 2 end: [-2.62606232598161;-1.7551311489536;0.148462995413926]
Robot Arm 1 togoal: 11.1461507714646
Step 268:
Mobile Platform 1 Pose: [-1.69832773908803;-3.98665270873308;-6.18853537085304]
Robot Arm 1 end: [-4.80976898549165;-6.27832320157649;0.633715281351937]
Robot Arm 1 togoal: 22.024993480829
Mobile Platform 2 Pose: [-1.33557420523117;-2.16502504462497;-2.0908639096055]
Robot Arm 2 end: [-2.6955551741483;-1.6802980099827;0.165760561689774]
Robot Arm 1 togoal: 11.1538795650554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.997725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-1.60249447415385;-4.05118655292879;-6.19322344068518]
Robot Arm 1 end: [-4.8780448904838;-6.21961291813536;0.688940151198361]
Robot Arm 1 togoal: 22.025539416082
Mobile Platform 2 Pose: [-1.21210429164104;-2.36565332049646;-1.98736315573941]
Robot Arm 2 end: [-2.81468219888675;-1.48531008576764;0.161330172427386]
Robot Arm 1 togoal: 11.2239671363799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.295625e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-1.46687781458445;-4.13694787067382;-6.19805935071625]
Robot Arm 1 end: [-4.94988199802739;-6.20501215746857;0.783054669323878]
Robot Arm 1 togoal: 22.0600533564566
Mobile Platform 2 Pose: [-1.53176211030694;-2.29771909438936;-1.88903382413641]
Robot Arm 2 end: [-2.8664430125589;-1.49613086912127;0.1950606543504]
Robot Arm 1 togoal: 11.1786198001598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.087195e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.149404e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-1.23509129170582;-4.27860710362836;-6.21517405465723]
Robot Arm 1 end: [-5.0238477655439;-6.06296261092103;0.929197508507028]
Robot Arm 1 togoal: 22.0019317268058
Mobile Platform 2 Pose: [-1.53503924975422;-2.40712844002294;-1.80222964066659]
Robot Arm 2 end: [-2.84486087054653;-1.2722222701762;0.168595274919739]
Robot Arm 1 togoal: 11.3665790193648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.627226e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.079966e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-0.957619565521787;-4.43829209258609;-6.21273162906102]
Robot Arm 1 end: [-5.07014551208324;-6.106919803937;1.10069954839248]
Robot Arm 1 togoal: 22.0617044626077
Mobile Platform 2 Pose: [-1.82806482806252;-2.33400426663863;-1.71676440468399]
Robot Arm 2 end: [-2.91928780979552;-1.30284115092796;0.218016003285774]
Robot Arm 1 togoal: 11.2904749440489

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.088931e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.354130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-0.57267302338172;-4.65389019316602;-6.46356884933236]
Robot Arm 1 end: [-5.093338107658;-6.29948354007051;1.2492706599282]
Robot Arm 1 togoal: 22.2161170265951
Mobile Platform 2 Pose: [-1.96099948497698;-2.37180226861385;-1.63170563084133]
Robot Arm 2 end: [-2.98871250982774;-1.28461840804827;0.195674737566256]
Robot Arm 1 togoal: 11.2637700444577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.558944e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.916869e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-0.38719216564755;-4.76255384186821;-6.46470928660637]
Robot Arm 1 end: [-5.10609004797175;-6.32815237060928;1.32337420426933]
Robot Arm 1 togoal: 22.2450347792581
Mobile Platform 2 Pose: [-2.06150861313305;-2.47134265433897;-1.53649898673316]
Robot Arm 2 end: [-3.04793002578054;-1.12448928247802;0.185228554455265]
Robot Arm 1 togoal: 11.3529843115378

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.589258e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-0.519102721480089;-4.68389563583396;-6.45918117853616]
Robot Arm 1 end: [-5.0966153865218;-6.23772157497001;1.27529693066369]
Robot Arm 1 togoal: 22.1727708704414
Mobile Platform 2 Pose: [-2.34231487204301;-2.42292273272683;-1.46385421713344]
Robot Arm 2 end: [-3.08975192332018;-1.08617951477474;0.311283102987502]
Robot Arm 1 togoal: 11.3433301589523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-0.621607535739471;-4.62426950060167;-6.44963922028318]
Robot Arm 1 end: [-5.09470616334416;-6.23412509330589;1.25463518518323]
Robot Arm 1 togoal: 22.1690758020576
Mobile Platform 2 Pose: [-2.36880164820825;-2.52363367041011;-1.39031380239023]
Robot Arm 2 end: [-3.14050034944325;-0.951499144713428;0.272354467228025]
Robot Arm 1 togoal: 11.423100229387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-0.689772364475892;-4.58685029661366;-6.4395102693133]
Robot Arm 1 end: [-5.11715464608575;-6.2407390210835;1.22305319252393]
Robot Arm 1 togoal: 22.1896088483935
Mobile Platform 2 Pose: [-2.614592723494;-2.50269388305509;-1.3152850336068]
Robot Arm 2 end: [-3.18911481548768;-0.927470965770826;0.362857899209827]
Robot Arm 1 togoal: 11.403518457001

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-0.723413626288418;-4.57385471727188;-6.43108333762384]
Robot Arm 1 end: [-5.18140955752226;-6.24949722171944;1.1575769658009]
Robot Arm 1 togoal: 22.2408055702504
Mobile Platform 2 Pose: [-2.63610687341889;-2.61371989492007;-1.2416191200492]
Robot Arm 2 end: [-3.25606139332582;-0.786262078677465;0.346339953087649]
Robot Arm 1 togoal: 11.4783927724756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-0.742133899913929;-4.59152346889848;-6.42343058620946]
Robot Arm 1 end: [-5.27643617343904;-6.28886272843391;1.05422811865377]
Robot Arm 1 togoal: 22.3363924378163
Mobile Platform 2 Pose: [-2.81884596253858;-2.63453225727858;-1.16217520255918]
Robot Arm 2 end: [-3.33022176132075;-0.771968813893084;0.384356899925004]
Robot Arm 1 togoal: 11.4426596061348

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-0.817477286828459;-4.57402669548009;-6.42152626347038]
Robot Arm 1 end: [-5.30450274208076;-6.36830483411219;0.948923643441807]
Robot Arm 1 togoal: 22.4159871226433
Mobile Platform 2 Pose: [-2.78677349195983;-2.7629327565878;-1.09653591441407]
Robot Arm 2 end: [-3.41713697439052;-0.641466968211437;0.37745521599013]
Robot Arm 1 togoal: 11.4988660655758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-0.855553403721264;-4.55882287953657;-6.42336096272315]
Robot Arm 1 end: [-5.30826658395255;-6.46891631959636;0.81496026566056]
Robot Arm 1 togoal: 22.4959268013943
Mobile Platform 2 Pose: [-2.79597240070465;-2.87481925476313;-1.01805755107387]
Robot Arm 2 end: [-3.40901068964068;-0.436679877551842;0.363613247307807]
Robot Arm 1 togoal: 11.6720938973691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-0.879784613554051;-4.54547097032909;-6.42218675389917]
Robot Arm 1 end: [-5.29074681835491;-6.57232245676269;0.673317705885391]
Robot Arm 1 togoal: 22.564737426917
Mobile Platform 2 Pose: [-2.75021533241911;-2.97048056192374;-0.949407061150039]
Robot Arm 2 end: [-3.45026104423825;-0.315174368032177;0.423659607456589]
Robot Arm 1 togoal: 11.7430440622247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-0.905512525277229;-4.53353554215361;-6.42085436152118]
Robot Arm 1 end: [-5.23992067413579;-6.6803912043066;0.545208412949303]
Robot Arm 1 togoal: 22.6151011959712
Mobile Platform 2 Pose: [-2.72856037866154;-3.0678658102881;-0.859157015435388]
Robot Arm 2 end: [-3.36863624041723;-0.127645277883362;0.471278648110852]
Robot Arm 1 togoal: 11.9390237451736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-0.900805277090918;-4.53948086089371;-6.42349574424685]
Robot Arm 1 end: [-5.25473232756699;-6.62393271301464;0.443161892186818]
Robot Arm 1 togoal: 22.5881325527191
Mobile Platform 2 Pose: [-2.73652484858534;-3.13417827025473;-0.753648653733175]
Robot Arm 2 end: [-3.35194170339079;-1.78674420450164e-05;0.519638330570412]
Robot Arm 1 togoal: 12.0498567194633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-0.861690192712627;-4.56686671543904;-6.4313713258661]
Robot Arm 1 end: [-5.27380589724266;-6.55203969498152;0.350464012304353]
Robot Arm 1 togoal: 22.5528117792301
Mobile Platform 2 Pose: [-2.93356302062648;-3.09679358544245;-0.638469755304736]
Robot Arm 2 end: [-3.37214829573471;0.0716144074998364;0.380019285797184]
Robot Arm 1 togoal: 12.1106429217145

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-0.759794563070324;-4.62722209393617;-6.44760364376528]
Robot Arm 1 end: [-5.29710314543461;-6.52555690788114;0.27092883471112]
Robot Arm 1 togoal: 22.5534492633825
Mobile Platform 2 Pose: [-3.03495898668529;-3.11277840568012;-0.529645175325507]
Robot Arm 2 end: [-3.30074548842675;0.176725643412294;0.295916238644968]
Robot Arm 1 togoal: 12.2452679045603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.732143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-0.603035879359133;-4.67882401581073;-6.50083568775529]
Robot Arm 1 end: [-5.3376143038085;-6.59534466202956;0.26087361556332]
Robot Arm 1 togoal: 22.6326267916187
Mobile Platform 2 Pose: [-3.16608478261276;-3.11327963440851;-0.406254536135161]
Robot Arm 2 end: [-3.32159476420586;0.226122825539671;0.177181949664914]
Robot Arm 1 togoal: 12.2874135394278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.371114e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-0.765401518711109;-4.45098563588128;-6.60374182296234]
Robot Arm 1 end: [-5.11450487421879;-6.77435173899938;0.315289950836802]
Robot Arm 1 togoal: 22.6115207489259
Mobile Platform 2 Pose: [-3.09543683289865;-3.21059347178778;-0.309560094594442]
Robot Arm 2 end: [-3.33697189344884;0.315132067128274;0.164669254001482]
Robot Arm 1 togoal: 12.3546560536313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.505460e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-0.928666965809251;-4.2588587966603;-6.62665523314716]
Robot Arm 1 end: [-4.90071820701135;-6.77170859696589;0.251599921540159]
Robot Arm 1 togoal: 22.4707140274957
Mobile Platform 2 Pose: [-3.19093980432903;-3.23894476778816;-0.158000644389522]
Robot Arm 2 end: [-3.42575346264955;0.465930416231041;0.18230644407544]
Robot Arm 1 togoal: 12.4317543994373

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010850e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-1.01966628325788;-4.12857404239217;-6.6430429784748]
Robot Arm 1 end: [-4.78245692313458;-6.77742791015109;0.277935464524148]
Robot Arm 1 togoal: 22.3952747115101
Mobile Platform 2 Pose: [-3.43815128727847;-3.16996537779091;-0.0499959465883809]
Robot Arm 2 end: [-3.51356020472256;0.496625542147949;0.158720006275196]
Robot Arm 1 togoal: 12.4140587952182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.644169e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-1.16200654387153;-3.98709838759005;-6.6502613416777]
Robot Arm 1 end: [-4.67595640369796;-6.77446376520233;0.331295229352462]
Robot Arm 1 togoal: 22.3199849555198
Mobile Platform 2 Pose: [-3.55383257725336;-3.18946351509955;0.0478758312904398]
Robot Arm 2 end: [-3.59180151928064;0.578370730561043;0.173921763846273]
Robot Arm 1 togoal: 12.441123536715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.762473e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-1.33406569206371;-3.78081128237114;-6.68285232832559]
Robot Arm 1 end: [-4.50114171741123;-6.77416344756026;0.407754284113438]
Robot Arm 1 togoal: 22.201238400095
Mobile Platform 2 Pose: [-3.697065892267;-3.20412740348662;0.142765542495782]
Robot Arm 2 end: [-3.60975342697272;0.66377605836025;0.152074543353538]
Robot Arm 1 togoal: 12.507011153461
Step 293:
Mobile Platform 1 Pose: [-1.35501368470408;-3.69594288667554;-6.73059265880757]
Robot Arm 1 end: [-4.39092585446294;-6.82056588602252;0.332840195473643]
Robot Arm 1 togoal: 22.1684890643401
Mobile Platform 2 Pose: [-3.86210244304647;-3.20156724517962;0.230258558738517]
Robot Arm 2 end: [-3.66992241039341;0.723071678433055;0.168856004831256]
Robot Arm 1 togoal: 12.5252721285776
Step 294:
Mobile Platform 1 Pose: [-1.60515104246397;-3.51672409392681;-6.71486518919349]
Robot Arm 1 end: [-4.24963046710596;-6.82161605391005;0.418657224017503]
Robot Arm 1 togoal: 22.0733752450775
Mobile Platform 2 Pose: [-4.00912125330372;-3.21706758075533;0.314073335423882]
Robot Arm 2 end: [-3.7165537572316;0.764772514395158;0.148982669605906]
Robot Arm 1 togoal: 12.5397151294333
Step 295:
Mobile Platform 1 Pose: [-1.48743968878978;-3.61644528866148;-6.70640342634721]
Robot Arm 1 end: [-4.36971472698841;-6.78256344596258;0.439287917982764]
Robot Arm 1 togoal: 22.1204258137399
Mobile Platform 2 Pose: [-4.15906221306078;-3.22922212909968;0.389801006160992]
Robot Arm 2 end: [-3.78449089367481;0.798892764529554;0.16700088828028]
Robot Arm 1 togoal: 12.5332338997227
Step 296:
Mobile Platform 1 Pose: [-1.40413761380595;-3.68036806004262;-6.70233638670802]
Robot Arm 1 end: [-4.4545745350127;-6.7637828715038;0.480918632421479]
Robot Arm 1 togoal: 22.1594429172384
Mobile Platform 2 Pose: [-4.25269993527432;-3.2880329314927;0.466034085586809]
Robot Arm 2 end: [-3.81356766834762;0.901305506303847;0.148027940815398]
Robot Arm 1 togoal: 12.6093401497139
Step 297:
Mobile Platform 1 Pose: [-1.33849638614977;-3.72774845266411;-6.70011692450404]
Robot Arm 1 end: [-4.51920084188828;-6.76007595230549;0.532501251513484]
Robot Arm 1 togoal: 22.1964967684069
Mobile Platform 2 Pose: [-4.4443433907238;-3.27680587577784;0.534400317409895]
Robot Arm 2 end: [-3.85321219798639;0.923506250047118;0.167426241107567]
Robot Arm 1 togoal: 12.6070675019437
Step 298:
Mobile Platform 1 Pose: [-1.28365409361728;-3.76556312175761;-6.69901539335584]
Robot Arm 1 end: [-4.57031151256886;-6.75943685933415;0.593172180397048]
Robot Arm 1 togoal: 22.2268658581936
Mobile Platform 2 Pose: [-4.51989596882652;-3.3416815558822;0.604316009706978]
Robot Arm 2 end: [-3.87130096939613;1.04203643313818;0.148198476659698]
Robot Arm 1 togoal: 12.7032320462535
Step 299:
Mobile Platform 1 Pose: [-1.23015213543467;-3.80081326026852;-6.69882162392429]
Robot Arm 1 end: [-4.61291386984787;-6.76062244368975;0.662841649188514]
Robot Arm 1 togoal: 22.2529133423598
Mobile Platform 2 Pose: [-4.71366645285537;-3.32532008215011;0.669006474322743]
Robot Arm 2 end: [-3.91114477897678;1.06475665016367;0.179221146034784]
Robot Arm 1 togoal: 12.7005262931451
Step 300:
Mobile Platform 1 Pose: [-1.16682790794317;-3.84073195414691;-6.70024273856607]
Robot Arm 1 end: [-4.65098950076647;-6.7633906579338;0.745376861832222]
Robot Arm 1 togoal: 22.2769915148912
Mobile Platform 2 Pose: [-4.80029159665169;-3.37857135618147;0.734654159476149]
Robot Arm 2 end: [-3.94366341585953;1.15781100644522;0.172968670335096]
Robot Arm 1 togoal: 12.7668769768376
Step 301:
Mobile Platform 1 Pose: [-1.06899268517087;-3.89982610286703;-6.70779780245475]
Robot Arm 1 end: [-4.68923974428813;-6.77107354412955;0.856123875692878]
Robot Arm 1 togoal: 22.3043657688007
Mobile Platform 2 Pose: [-4.96880441660253;-3.37705163725553;0.795876108725435]
Robot Arm 2 end: [-3.99726027572067;1.17184670042962;0.205624936353098]
Robot Arm 1 togoal: 12.7504217362958
Step 302:
Mobile Platform 1 Pose: [-0.940356369674494;-3.96084632674594;-6.80020397157169]
Robot Arm 1 end: [-4.69223687640994;-6.89250353533228;1.00322862439403]
Robot Arm 1 togoal: 22.3938962068049
Mobile Platform 2 Pose: [-5.02497294467504;-3.44462988773548;0.854894355485065]
Robot Arm 2 end: [-4.02287067231474;1.28911753379686;0.197444082495789]
Robot Arm 1 togoal: 12.842198018973
Step 303:
Mobile Platform 1 Pose: [-0.995020783894069;-3.92677172383553;-6.80040470016992]
Robot Arm 1 end: [-4.67728507838441;-6.89725154951307;0.996646541477311]
Robot Arm 1 togoal: 22.3878260165911
Mobile Platform 2 Pose: [-5.17972887415854;-3.44758878262411;0.91353706451089]
Robot Arm 2 end: [-4.08076054513656;1.29824844099273;0.229817528408559]
Robot Arm 1 togoal: 12.8201000672129
Step 304:
Mobile Platform 1 Pose: [-1.03140309915343;-3.90433445176612;-6.79749937169637]
Robot Arm 1 end: [-4.67767372929193;-6.90909595552878;0.968191105726274]
Robot Arm 1 togoal: 22.3977059050791
Mobile Platform 2 Pose: [-5.2187993423653;-3.51620007299;0.967472069087958]
Robot Arm 2 end: [-4.10725720260709;1.41091499590909;0.22267433494621]
Robot Arm 1 togoal: 12.9081001023631
Step 305:
Mobile Platform 1 Pose: [-1.04647187620885;-3.89682280373184;-6.79317783802558]
Robot Arm 1 end: [-4.69699852302673;-6.92343935170262;0.914212701678915]
Robot Arm 1 togoal: 22.4225947629174
Mobile Platform 2 Pose: [-5.33795646584861;-3.53809585948138;1.02577150371869]
Robot Arm 2 end: [-4.17773361390916;1.41279208049716;0.23852077529179]
Robot Arm 1 togoal: 12.8761547794778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.635164e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-1.06832432113223;-3.90058178727438;-6.78699626595112]
Robot Arm 1 end: [-4.72664966130982;-6.93456327522601;0.834991779528996]
Robot Arm 1 togoal: 22.4527114364422
Mobile Platform 2 Pose: [-5.32625915411992;-3.61859587552137;1.07073534265825]
Robot Arm 2 end: [-4.21596807956655;1.51191341934696;0.22794876618563]
Robot Arm 1 togoal: 12.9480175055644

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-1.12529440544223;-3.88274375644712;-6.7808272447001]
Robot Arm 1 end: [-4.72658540147985;-6.93648655948941;0.736650252149662]
Robot Arm 1 togoal: 22.4573395373049
Mobile Platform 2 Pose: [-5.32616525827879;-3.69553812014257;1.12303198468277]
Robot Arm 2 end: [-4.15372499581409;1.70302513999726;0.222051632644972]
Robot Arm 1 togoal: 13.1463681567584

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-1.18637595678746;-3.86174940225307;-6.77691678166569]
Robot Arm 1 end: [-4.70634326043766;-6.92031450490935;0.653793861704582]
Robot Arm 1 togoal: 22.4349196906332
Mobile Platform 2 Pose: [-5.5111823306232;-3.67281533424193;1.19100246827554]
Robot Arm 2 end: [-4.25072268508454;1.64261574083267;0.208666518117435]
Robot Arm 1 togoal: 13.0509352193686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-1.20697514656653;-3.85307135697275;-6.77098745671477]
Robot Arm 1 end: [-4.68780036091847;-6.89830885139261;0.530086253255391]
Robot Arm 1 togoal: 22.4112969623076
Mobile Platform 2 Pose: [-5.50646150394501;-3.74991983314658;1.24257183206273]
Robot Arm 2 end: [-4.18849560179388;1.83029486479915;0.211640069342185]
Robot Arm 1 togoal: 13.2455231450658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-1.25499189533205;-3.83527097993912;-6.76358185719546]
Robot Arm 1 end: [-4.63782760513494;-6.87850761134485;0.397010720343982]
Robot Arm 1 togoal: 22.3699210567373
Mobile Platform 2 Pose: [-5.61229397694155;-3.77259183919913;1.3171828557127]
Robot Arm 2 end: [-4.17036467462676;1.92076899808822;0.194553263628459]
Robot Arm 1 togoal: 13.3359979932906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-1.32173039322145;-3.81742188093918;-6.75584602142671]
Robot Arm 1 end: [-4.58453516721308;-6.87908458992214;0.30996928965351]
Robot Arm 1 togoal: 22.3400706807034
Mobile Platform 2 Pose: [-5.67519248208983;-3.79297684083935;1.37651195017206]
Robot Arm 2 end: [-4.04480219132129;2.04443179729308;0.182947447732444]
Robot Arm 1 togoal: 13.5027072430963

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-1.36392756334913;-3.83692074481686;-6.75154221003883]
Robot Arm 1 end: [-4.57379864044745;-6.90890901511904;0.265453877156774]
Robot Arm 1 togoal: 22.3580680507165
Mobile Platform 2 Pose: [-5.73073693836469;-3.79805729528045;1.43013370433382]
Robot Arm 2 end: [-3.9691228060973;2.10559195519811;0.162147465012253]
Robot Arm 1 togoal: 13.5927670665518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-1.26232194682257;-3.92443900460082;-6.76435549661632]
Robot Arm 1 end: [-4.63644855826815;-6.94167519146132;0.256760631869827]
Robot Arm 1 togoal: 22.4241897483886
Mobile Platform 2 Pose: [-5.79046087336686;-3.81183094396937;1.50822520524636]
Robot Arm 2 end: [-3.84195058488641;2.19636040458229;0.154787922436248]
Robot Arm 1 togoal: 13.7309619167167

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-1.13030029668937;-4.00664199020496;-6.78216671645694]
Robot Arm 1 end: [-4.71053653126992;-6.95632697836005;0.273076129945563]
Robot Arm 1 togoal: 22.4827539153979
Mobile Platform 2 Pose: [-5.92480706750592;-3.77213361229037;1.58413544060237]
Robot Arm 2 end: [-3.7627277728093;2.26702583348421;0.139787323648662]
Robot Arm 1 togoal: 13.8308074996642

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.241535e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-0.968428677194564;-4.0999663616006;-6.80684218558856]
Robot Arm 1 end: [-4.79177961652135;-6.98175886709306;0.280354506058561]
Robot Arm 1 togoal: 22.5547351943105
Mobile Platform 2 Pose: [-6.11809879766039;-3.6867732409102;1.63417727796411]
Robot Arm 2 end: [-3.77257437573943;2.31477948978082;0.133936340031251]
Robot Arm 1 togoal: 13.8693391834085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.915801e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-0.770761962465076;-4.20517620909664;-6.84318412546659]
Robot Arm 1 end: [-4.88614060339654;-7.01824760194463;0.285566243647851]
Robot Arm 1 togoal: 22.6438563427593
Mobile Platform 2 Pose: [-6.28658137635969;-3.61390569023995;1.6725488446906]
Robot Arm 2 end: [-3.79337834346742;2.33033076597216;0.123595010553159]
Robot Arm 1 togoal: 13.8748675629769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.282530e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-0.535510272688756;-4.32025887733655;-6.89773547760559]
Robot Arm 1 end: [-4.99227096756353;-7.07231227014542;0.289728191469979]
Robot Arm 1 togoal: 22.7540779923223
Mobile Platform 2 Pose: [-6.44643121315628;-3.55329956667784;1.71670790616785]
Robot Arm 2 end: [-3.76870240255107;2.39645018098389;0.161513930233665]
Robot Arm 1 togoal: 13.9409199557087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.198265e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-0.276265130485328;-4.43442746336774;-6.97626680856607]
Robot Arm 1 end: [-5.09917294906587;-7.14976935421539;0.293133167240851]
Robot Arm 1 togoal: 22.8824664231716
Mobile Platform 2 Pose: [-6.5864628182635;-3.49179802241938;1.74007660615985]
Robot Arm 2 end: [-3.74630566224928;2.41470010325939;0.171361001250187]
Robot Arm 1 togoal: 13.9662091116677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.716533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-0.0196562783548679;-4.53103477382835;-7.08120634898173]
Robot Arm 1 end: [-5.18816633655257;-7.25499741419299;0.29375385858074]
Robot Arm 1 togoal: 23.0200231433903
Mobile Platform 2 Pose: [-6.72686608334834;-3.43120343350225;1.76322052216419]
Robot Arm 2 end: [-3.73370388074052;2.43456809189022;0.194696461757819]
Robot Arm 1 togoal: 13.9872810710894

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.232827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [0.185593807104958;-4.5752074996325;-7.21742534817696]
Robot Arm 1 end: [-5.23049784888621;-7.39454755182372;0.291921744048639]
Robot Arm 1 togoal: 23.1527316448154
Mobile Platform 2 Pose: [-6.86396654661363;-3.37314129624446;1.78524723940078]
Robot Arm 2 end: [-3.72714030321395;2.45518983080823;0.232233744241136]
Robot Arm 1 togoal: 14.0050505518193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.723545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [0.362291757354161;-4.59258986490955;-7.36084609772726]
Robot Arm 1 end: [-5.24215877706144;-7.54477795400716;0.285060716001233]
Robot Arm 1 togoal: 23.2738089610252
Mobile Platform 2 Pose: [-6.99671799915741;-3.31801639084848;1.80588797454507]
Robot Arm 2 end: [-3.67262072704191;2.49767436861878;0.272012773428289]
Robot Arm 1 togoal: 14.0636966043793

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.511140e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [0.42513040615649;-4.52179214275434;-7.51134768391541]
Robot Arm 1 end: [-5.1774513763604;-7.68804467006001;0.291820103389137]
Robot Arm 1 togoal: 23.3395123717282
Mobile Platform 2 Pose: [-7.12240436884763;-3.26232949119654;1.82088860518406]
Robot Arm 2 end: [-3.57776655069667;2.56955572885174;0.362567822962004]
Robot Arm 1 togoal: 14.162654370098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.598875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [0.529763872828424;-4.47337008974148;-7.66337555805062]
Robot Arm 1 end: [-5.08433039053689;-7.86939613309999;0.238292303164566]
Robot Arm 1 togoal: 23.4200273383283
Mobile Platform 2 Pose: [-7.24084416773547;-3.207657961817;1.83290434919811]
Robot Arm 2 end: [-3.44503405803291;2.63674904390681;0.46130785529131]
Robot Arm 1 togoal: 14.2750815178263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978234e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [0.267965887632562;-4.26127826858864;-7.68861443368205]
Robot Arm 1 end: [-4.90655968291978;-7.86791793988918;0.273654525792038]
Robot Arm 1 togoal: 23.3028728662174
Mobile Platform 2 Pose: [-7.3540174274006;-3.15368879366056;1.84311335628602]
Robot Arm 2 end: [-3.39298126156296;2.6571472017212;0.539046085280571]
Robot Arm 1 togoal: 14.3115444794309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.454919e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [0.0682333178182036;-4.06428992535173;-7.71689164575948]
Robot Arm 1 end: [-4.66907377201778;-7.91035186255844;0.223872512498864]
Robot Arm 1 togoal: 23.1871725074098
Mobile Platform 2 Pose: [-7.46711487934674;-3.1000606052615;1.85366501785825]
Robot Arm 2 end: [-3.52194515790291;2.57264379600491;0.494790551403959]
Robot Arm 1 togoal: 14.1806090275004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.894245e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [-0.0846661577535411;-3.92630149974407;-7.7183332096225]
Robot Arm 1 end: [-4.56032414451135;-7.89472358816898;0.253429737385231]
Robot Arm 1 togoal: 23.1047810173755
Mobile Platform 2 Pose: [-7.59772477355132;-3.05409392886707;1.87978679925689]
Robot Arm 2 end: [-3.56502310921656;2.51302188557095;0.406802834898031]
Robot Arm 1 togoal: 14.1147468846032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [-0.131476849320296;-3.82518424780645;-7.73314045585469]
Robot Arm 1 end: [-4.44622756735312;-7.90738155271662;0.233358336739299]
Robot Arm 1 togoal: 23.0439976144612
Mobile Platform 2 Pose: [-7.58041192396771;-3.09571127040737;1.91051535913509]
Robot Arm 2 end: [-3.46826956092758;2.63745132455037;0.45661909770772]
Robot Arm 1 togoal: 14.2653981793096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.349542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [-0.178389372189336;-3.73212029458522;-7.74438051388461]
Robot Arm 1 end: [-4.35665485099152;-7.90950390477332;0.233323812539357]
Robot Arm 1 togoal: 22.9896058815575
Mobile Platform 2 Pose: [-7.6530606889934;-3.12269320331422;1.98141889084061]
Robot Arm 2 end: [-3.57788845378517;2.59943466495734;0.309989995878397]
Robot Arm 1 togoal: 14.193513159843

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.340562e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [-0.217508243993983;-3.64490561989559;-7.75685830476356]
Robot Arm 1 end: [-4.25962694789593;-7.91101379253479;0.215589109246091]
Robot Arm 1 togoal: 22.9313207941285
Mobile Platform 2 Pose: [-7.64733063012914;-3.13217024340544;1.98870231120784]
Robot Arm 2 end: [-3.47459806428956;2.6880199998012;0.384080998556882]
Robot Arm 1 togoal: 14.3129069987117
Step 330:
Mobile Platform 1 Pose: [-0.309239573961757;-3.53993643396078;-7.76275617268169]
Robot Arm 1 end: [-4.16072758120567;-7.9034087403876;0.219564327575044]
Robot Arm 1 togoal: 22.8637701388893
Mobile Platform 2 Pose: [-7.61429573198133;-3.16275632975205;2.00515524327485]
Robot Arm 2 end: [-3.3716088890965;2.76383848082226;0.509015571095067]
Robot Arm 1 togoal: 14.4178796626955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.415814e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-0.420881644678709;-3.433729678711;-7.76505368193231]
Robot Arm 1 end: [-4.03154515076551;-7.87839375163337;0.184260713329262]
Robot Arm 1 togoal: 22.7663807922327
Mobile Platform 2 Pose: [-7.58923692299615;-3.21865168006385;2.06973988105363]
Robot Arm 2 end: [-3.28974280494092;2.84387993052432;0.606153032153146]
Robot Arm 1 togoal: 14.5199775574257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.651142e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-0.482669716337805;-3.38352050556101;-7.76212416836305]
Robot Arm 1 end: [-3.97682502843962;-7.84622828485088;0.175222111089933]
Robot Arm 1 togoal: 22.7079417824889
Mobile Platform 2 Pose: [-7.75076315122034;-3.17147105199868;2.14676564152901]
Robot Arm 2 end: [-3.25145095764437;2.90996773763515;0.455148434039844]
Robot Arm 1 togoal: 14.6063760298206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.982575e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-0.487151767542733;-3.38241437462161;-7.75680869576238]
Robot Arm 1 end: [-3.98214250125423;-7.80246892633231;0.186625909354326]
Robot Arm 1 togoal: 22.6761717009771
Mobile Platform 2 Pose: [-7.85304036583651;-3.15816888439096;2.22797371545507]
Robot Arm 2 end: [-3.160421254892;2.95933633256837;0.333955295175802]
Robot Arm 1 togoal: 14.7014730800152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006933e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [-0.455730929922599;-3.41246103499751;-7.74947842512298]
Robot Arm 1 end: [-4.03448648087208;-7.74098895206598;0.240495112096864]
Robot Arm 1 togoal: 22.6572106082601
Mobile Platform 2 Pose: [-7.90368342333687;-3.16765254209953;2.31257037640362]
Robot Arm 2 end: [-3.05555496872255;3.0179158916048;0.30373750649523]
Robot Arm 1 togoal: 14.8044986045264

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007872e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [-0.418374484151377;-3.44300836703067;-7.74325225146568]
Robot Arm 1 end: [-4.09961475574435;-7.67900773958404;0.344554752198088]
Robot Arm 1 togoal: 22.6435525997524
Mobile Platform 2 Pose: [-8.06627724699043;-3.10646629239931;2.39610244238361]
Robot Arm 2 end: [-2.96968379614481;3.03995639458125;0.187389915121069]
Robot Arm 1 togoal: 14.8742840014994

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003370e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [-0.406473622999562;-3.451679744341;-7.73872209609629]
Robot Arm 1 end: [-4.11467201920479;-7.61760376962542;0.44206747611944]
Robot Arm 1 togoal: 22.6001775255047
Mobile Platform 2 Pose: [-8.23332287752769;-3.03003307039592;2.44700575932604]
Robot Arm 2 end: [-2.88029242775126;3.08070290428785;0.161335393834192]
Robot Arm 1 togoal: 14.9547220711667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002526e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [-0.401990298908404;-3.45512970112861;-7.7342669617481]
Robot Arm 1 end: [-4.11921812263914;-7.56692742050895;0.574279987217902]
Robot Arm 1 togoal: 22.5576048855966
Mobile Platform 2 Pose: [-8.41923479177725;-2.93896676364992;2.48425759572226]
Robot Arm 2 end: [-2.85420081097643;3.14085580376324;0.164451170088926]
Robot Arm 1 togoal: 15.0194794195099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003407e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [-0.407125229763417;-3.45295080427846;-7.72684697403351]
Robot Arm 1 end: [-4.12307145652984;-7.5512141582244;0.712960428580336]
Robot Arm 1 togoal: 22.5423866026445
Mobile Platform 2 Pose: [-8.58465187358625;-2.85870407776606;2.51145926840534]
Robot Arm 2 end: [-2.85690724089237;3.17434619499815;0.159106840385595]
Robot Arm 1 togoal: 15.0479874006155

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007222e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [-0.387118500653872;-3.46726699100544;-7.71322112599992]
Robot Arm 1 end: [-4.15250798641352;-7.57468353203075;0.8354354162801]
Robot Arm 1 togoal: 22.5750522115671
Mobile Platform 2 Pose: [-8.74809806990203;-2.78297050382343;2.53961080766487]
Robot Arm 2 end: [-2.88854790739037;3.19581349064531;0.156601847919364]
Robot Arm 1 togoal: 15.0520372994083

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005393e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [-0.315207725316177;-3.51244426185886;-7.70209698784498]
Robot Arm 1 end: [-4.17947675174724;-7.57518016227349;0.946593373583093]
Robot Arm 1 togoal: 22.5892554472737
Mobile Platform 2 Pose: [-8.91083647391352;-2.71320122982646;2.57011426961405]
Robot Arm 2 end: [-2.93927750649899;3.19691496985208;0.155712414078]
Robot Arm 1 togoal: 15.0291841775929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002643e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [-0.236841962847513;-3.55944257684575;-7.69621246895141]
Robot Arm 1 end: [-4.16270870921291;-7.54733471942811;1.06782528384542]
Robot Arm 1 togoal: 22.554304269053
Mobile Platform 2 Pose: [-9.0717327032828;-2.65285884284826;2.60444193394727]
Robot Arm 2 end: [-2.98956513395854;3.18154400121595;0.156486441364445]
Robot Arm 1 togoal: 14.9920377707202

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001592e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.257945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [-0.152979482547826;-3.60821543501919;-7.69221917843425]
Robot Arm 1 end: [-4.12363915354651;-7.5170219923212;1.17530153486781]
Robot Arm 1 togoal: 22.5042829223206
Mobile Platform 2 Pose: [-9.22527171206789;-2.60889290421949;2.64489936078053]
Robot Arm 2 end: [-3.04399004556836;3.1569873036443;0.160346726106313]
Robot Arm 1 togoal: 14.9446902519312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.927710e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [-0.0747439938670759;-3.65452150672997;-7.68976159197577]
Robot Arm 1 end: [-4.06257581899816;-7.53292449782721;1.29471116125529]
Robot Arm 1 togoal: 22.4768922840797
Mobile Platform 2 Pose: [-9.36642929751643;-2.58942169140423;2.69275574808279]
Robot Arm 2 end: [-3.06058787224693;3.17565442509764;0.160621080731896]
Robot Arm 1 togoal: 14.9533922911288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.240851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000414e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [0.0806932036681483;-3.74985037276144;-7.70370635485356]
Robot Arm 1 end: [-4.00194034266564;-7.53579711069889;1.38132913136713]
Robot Arm 1 togoal: 22.440544147117
Mobile Platform 2 Pose: [-9.49309824685535;-2.58400971842928;2.74190207802625]
Robot Arm 2 end: [-2.92947951187569;3.33633282707642;0.17369440124008]
Robot Arm 1 togoal: 15.1547110043046

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.965602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000847e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [0.35986410527595;-3.91152162274448;-7.70728221336477]
Robot Arm 1 end: [-3.99141819384027;-7.59649035184897;1.43929535206709]
Robot Arm 1 togoal: 22.4811683732659
Mobile Platform 2 Pose: [-9.63005876661062;-2.53283667123193;2.76589543666493]
Robot Arm 2 end: [-2.78350893187459;3.40634736866138;0.170229575305234]
Robot Arm 1 togoal: 15.2850408519499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.452748e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000945e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [0.722161067662541;-4.11035926239181;-7.69343877797265]
Robot Arm 1 end: [-3.90998410700459;-7.65011389981503;1.43158993204323]
Robot Arm 1 togoal: 22.4726983847916
Mobile Platform 2 Pose: [-9.75388533105663;-2.47926080957098;2.78273788924154]
Robot Arm 2 end: [-2.63348282135392;3.45631027726469;0.169952316178644]
Robot Arm 1 togoal: 15.4001036186536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.646434e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000880e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [1.33991848265198;-4.30479103447178;-7.54213556470701]
Robot Arm 1 end: [-3.96161440440162;-7.52852482968224;1.39179702110771]
Robot Arm 1 togoal: 22.4096531185053
Mobile Platform 2 Pose: [-9.85314650092435;-2.433800878368;2.79612268380017]
Robot Arm 2 end: [-2.50099983465656;3.48676420374753;0.168292169156451]
Robot Arm 1 togoal: 15.4905658593523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.062451e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000729e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [1.60018906524095;-4.25309611623649;-7.37041238364351]
Robot Arm 1 end: [-3.78435817836678;-7.32614273862211;1.34265728400098]
Robot Arm 1 togoal: 22.1412637108401
Mobile Platform 2 Pose: [-9.95732813550664;-2.38399892728833;2.80867280316671]
Robot Arm 2 end: [-2.38563486331809;3.50394062537105;0.16457628850377]
Robot Arm 1 togoal: 15.5619681960018

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.005185e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000522e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [1.25110346757972;-4.03115456007882;-7.34180188674496]
Robot Arm 1 end: [-3.69698308119287;-7.18942970136923;1.37186904971916]
Robot Arm 1 togoal: 21.979674224727
Mobile Platform 2 Pose: [-10.0704373781433;-2.32880826817102;2.82175537188902]
Robot Arm 2 end: [-2.29371225168561;3.51566246900168;0.15936388740504]
Robot Arm 1 togoal: 15.6177481007642

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.491445e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000269e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [1.62857887248282;-4.18212915238739;-7.26933161591453]
Robot Arm 1 end: [-3.72210477125055;-7.10842128850825;1.33233813070447]
Robot Arm 1 togoal: 21.9323922917493
Mobile Platform 2 Pose: [-10.1937668137041;-2.26816942642001;2.8361128536436]
Robot Arm 2 end: [-2.23953772162029;3.52863530037356;0.15444368538737]
Robot Arm 1 togoal: 15.6561880426136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.601762e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000151e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [1.89964926820456;-4.27060279654741;-7.19693059661151]
Robot Arm 1 end: [-3.74973943362533;-6.84589405177018;1.22680229153882]
Robot Arm 1 togoal: 21.7468626056202
Mobile Platform 2 Pose: [-10.320976850496;-2.20589766441348;2.85134275026436]
Robot Arm 2 end: [-2.2094699244869;3.54335005042758;0.151510998690147]
Robot Arm 1 togoal: 15.6840743931735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.319357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000047e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [1.93676778492212;-4.17986405285856;-7.03560940306064]
Robot Arm 1 end: [-3.6103713353226;-6.64864372148643;1.17128490139229]
Robot Arm 1 togoal: 21.5067860523267
Mobile Platform 2 Pose: [-10.4498617713675;-2.14372052024998;2.86746861226257]
Robot Arm 2 end: [-2.19331757387065;3.55593177140575;0.148640970240021]
Robot Arm 1 togoal: 15.7032145116854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.579905e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [1.69526865152927;-4.00817752707115;-6.97147798996556]
Robot Arm 1 end: [-3.52266373140307;-6.60868673658416;1.24619952904645]
Robot Arm 1 togoal: 21.4193002874843
Mobile Platform 2 Pose: [-10.5806776238037;-2.08222858840742;2.88521067094157]
Robot Arm 2 end: [-2.21445744345871;3.56048078539075;0.146247188183016]
Robot Arm 1 togoal: 15.6968550138002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.183220e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [1.43499721303691;-3.79302104205822;-6.87981949332871]
Robot Arm 1 end: [-3.52327891325432;-6.35316621736241;1.22505003593428]
Robot Arm 1 togoal: 21.2224575457128
Mobile Platform 2 Pose: [-10.7154015349341;-2.0226408211716;2.90629951501877]
Robot Arm 2 end: [-2.28738449553287;3.53662118620644;0.142758124189036]
Robot Arm 1 togoal: 15.640463603322

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.703094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997734e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [1.68685194970483;-3.83043845143998;-6.73950658048206]
Robot Arm 1 end: [-3.32975819827431;-6.33389506741989;1.15420397730477]
Robot Arm 1 togoal: 21.085851204545
Mobile Platform 2 Pose: [-10.8611004585313;-1.96922904612327;2.93661508514005]
Robot Arm 2 end: [-2.32644485682546;3.527617396057;0.150437049726214]
Robot Arm 1 togoal: 15.6127615670677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [1.46230080341807;-3.6758966869165;-6.70034858277389]
Robot Arm 1 end: [-3.34910050775047;-6.03336210987049;1.04032972471829]
Robot Arm 1 togoal: 20.8685943183961
Mobile Platform 2 Pose: [-10.9999042039951;-1.93443812457429;2.97691022515421]
Robot Arm 2 end: [-2.38321337913187;3.50680464307588;0.155121358623439]
Robot Arm 1 togoal: 15.5664650629814

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992395e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [1.69237271781701;-3.68648599729844;-6.53202216385479]
Robot Arm 1 end: [-3.27923063993188;-6.04308381563208;1.16307991974301]
Robot Arm 1 togoal: 20.8289848885574
Mobile Platform 2 Pose: [-11.0810476787042;-1.95322879055276;3.02919276083228]
Robot Arm 2 end: [-2.49479921350805;3.4655702378005;0.154906037110421]
Robot Arm 1 togoal: 15.4762990857033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009582e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993109e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [1.44227462869658;-3.51958520730472;-6.49214932179741]
Robot Arm 1 end: [-3.2007335885164;-5.82178031180976;1.04070910358555]
Robot Arm 1 togoal: 20.6111324010918
Mobile Platform 2 Pose: [-11.0347863173026;-2.05006582086855;3.07408736899385]
Robot Arm 2 end: [-2.50640580104861;3.60417845023351;0.158395428228382]
Robot Arm 1 togoal: 15.5911614879511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000087e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997261e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [1.53710323126555;-3.42391457042607;-6.2840014121923]
Robot Arm 1 end: [-3.08615227159846;-5.74492288563733;1.16288361578451]
Robot Arm 1 togoal: 20.4762844601937
Mobile Platform 2 Pose: [-11.0056707340638;-2.11697792675024;3.11672697205591]
Robot Arm 2 end: [-2.4807981846825;3.71087930539988;0.157270308078023]
Robot Arm 1 togoal: 15.6966992052261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011235e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [1.25262248931724;-3.23102582274819;-6.23025984163034]
Robot Arm 1 end: [-3.00741463742323;-5.52196250547786;1.040050101917]
Robot Arm 1 togoal: 20.2572324954762
Mobile Platform 2 Pose: [-11.0003676671714;-2.18483042929916;3.17891985743673]
Robot Arm 2 end: [-2.5402564747079;3.77551324541736;0.158907253400558]
Robot Arm 1 togoal: 15.7247523124577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007019e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000070e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [1.50213197590152;-3.2505031100052;-6.04743167685793]
Robot Arm 1 end: [-2.94067090094889;-5.53456904787613;1.20538405190641]
Robot Arm 1 togoal: 20.2208657652581
Mobile Platform 2 Pose: [-10.9629274998342;-2.22232806627926;3.19776232154286]
Robot Arm 2 end: [-2.46246878635848;3.83885661863701;0.160266956780954]
Robot Arm 1 togoal: 15.8170769651893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005211e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000282e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [1.27877136417833;-3.11134370834612;-6.02122988137004]
Robot Arm 1 end: [-2.92892588038173;-5.33674463210879;1.09772404798565]
Robot Arm 1 togoal: 20.0636991320762
Mobile Platform 2 Pose: [-10.8798506418511;-2.2957149451839;3.23024037113585]
Robot Arm 2 end: [-2.41995655998827;3.88350072677714;0.16608400063845]
Robot Arm 1 togoal: 15.8759077960352

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007483e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000308e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [1.04618271926667;-2.96467595771246;-5.98933384227013]
Robot Arm 1 end: [-2.66477351865569;-5.26435122923544;0.996061212423023]
Robot Arm 1 togoal: 19.8411811004034
Mobile Platform 2 Pose: [-10.8585823782026;-2.31116600979707;3.23730487570303]
Robot Arm 2 end: [-2.30873585515891;3.88458202613414;0.171900839033105]
Robot Arm 1 togoal: 15.929759280425

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [0.79253924151068;-2.77885985936498;-5.92169766335469]
Robot Arm 1 end: [-2.26463479999005;-5.25894485286411;0.845724747868382]
Robot Arm 1 togoal: 19.5885849786557
Mobile Platform 2 Pose: [-10.8300585227179;-2.32939796856424;3.24353286062425]
Robot Arm 2 end: [-2.19176468843359;3.88348106439151;0.219365182069338]
Robot Arm 1 togoal: 15.9816719508742

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000192e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.861818e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [0.720885874415179;-2.73639282229087;-5.91983139302105]
Robot Arm 1 end: [-2.1160332650343;-5.42877718177717;0.940426588107305]
Robot Arm 1 togoal: 19.6260666688703
Mobile Platform 2 Pose: [-10.7802347020267;-2.36018535187444;3.25263353053906]
Robot Arm 2 end: [-2.06939991481575;3.89493910179051;0.35022438067992]
Robot Arm 1 togoal: 16.0416478550174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010596e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.856039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [0.640365516607597;-2.68743721216301;-5.91759418908461]
Robot Arm 1 end: [-2.01265146623124;-5.6138562232578;1.00470695048033]
Robot Arm 1 togoal: 19.7069241618954
Mobile Platform 2 Pose: [-10.5918183803757;-2.49264253965631;3.37950184581242]
Robot Arm 2 end: [-2.01580615733264;3.93927821061028;0.542162910208424]
Robot Arm 1 togoal: 16.0937550123777

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993070e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.678231e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [0.551807387797035;-2.63208616745349;-5.91497989421405]
Robot Arm 1 end: [-1.96685738021908;-5.78291434830338;1.07049750306166]
Robot Arm 1 togoal: 19.8118402321473
Mobile Platform 2 Pose: [-11.0054612865839;-2.27015944453278;3.49544694963795]
Robot Arm 2 end: [-1.75847574567787;3.98960781013989;0.366058802752011]
Robot Arm 1 togoal: 16.2777682095921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978609e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [0.439692295617714;-2.55861504859663;-5.91001288449405]
Robot Arm 1 end: [-1.98919743917761;-5.86395416705507;1.18326305929271]
Robot Arm 1 togoal: 19.887683708545
Mobile Platform 2 Pose: [-10.7667418715603;-2.40632349939077;3.51764527487638]
Robot Arm 2 end: [-1.7636448826483;3.90722490213483;0.508967968998924]
Robot Arm 1 togoal: 16.1948343285585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000789e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [0.238436413351645;-2.42183611585405;-5.88467021048741]
Robot Arm 1 end: [-2.01421820086923;-6.03823580837866;1.27350432601769]
Robot Arm 1 togoal: 20.040651210275
Mobile Platform 2 Pose: [-10.9953034444142;-2.2718996121702;3.54033988882284]
Robot Arm 2 end: [-1.63418195181877;3.8705884611113;0.353046904730153]
Robot Arm 1 togoal: 16.2402047494956

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [-0.0312767230863623;-2.20517775540578;-5.75993386915623]
Robot Arm 1 end: [-1.89649737366366;-6.06355762045388;1.27799816318809]
Robot Arm 1 togoal: 19.9905873620542
Mobile Platform 2 Pose: [-11.2577458851377;-2.11422464028179;3.56463442105372]
Robot Arm 2 end: [-1.49458723816792;3.84976521891688;0.252892654868763]
Robot Arm 1 togoal: 16.3023353881287

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [0.0143691768363034;-2.15010238544389;-5.53896641455257]
Robot Arm 1 end: [-1.69452552259806;-5.76817196042785;1.23649864754422]
Robot Arm 1 togoal: 19.6335965933873
Mobile Platform 2 Pose: [-11.4257974587607;-2.01430692744823;3.57302761588099]
Robot Arm 2 end: [-1.36301878062674;3.81442432496562;0.266245389839566]
Robot Arm 1 togoal: 16.3404459286069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003351e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [0.0766857249019339;-2.08915221612304;-5.35301805281131]
Robot Arm 1 end: [-1.64531615602889;-5.50312666412166;1.26891706174952]
Robot Arm 1 togoal: 19.3913349345893
Mobile Platform 2 Pose: [-11.6073434703144;-1.90402784785798;3.58175722237969]
Robot Arm 2 end: [-1.22813252875708;3.82022526826091;0.302161483300648]
Robot Arm 1 togoal: 16.4143185276854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000094e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [-0.22475657979965;-1.86437810808994;-5.28731997859727]
Robot Arm 1 end: [-1.58582082641192;-5.50791032415397;1.31886482523051]
Robot Arm 1 togoal: 19.3589097180756
Mobile Platform 2 Pose: [-11.7924250883211;-1.78935922527403;3.59407573562262]
Robot Arm 2 end: [-1.09387904947087;3.86026594895603;0.357051444204362]
Robot Arm 1 togoal: 16.5160751633608

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978634e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [-0.228624579232633;-1.82525332850563;-5.08427831524845]
Robot Arm 1 end: [-1.54366270969738;-5.28041706599728;1.32458949196069]
Robot Arm 1 togoal: 19.1516545382833
Mobile Platform 2 Pose: [-11.9854696153458;-1.66724050770551;3.61492117593331]
Robot Arm 2 end: [-0.961071829251741;3.93074586531972;0.427989933260677]
Robot Arm 1 togoal: 16.6421925792866

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [-0.199485779065417;-1.86356915876323;-4.99636716193776]
Robot Arm 1 end: [-1.5949278566706;-5.09839780597039;1.34250708159761]
Robot Arm 1 togoal: 19.0377474515933
Mobile Platform 2 Pose: [-12.183311548776;-1.53989894772627;3.65220170970265]
Robot Arm 2 end: [-0.826219820885444;4.01445364435197;0.522084636450296]
Robot Arm 1 togoal: 16.7797790697

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000842e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [-0.464192562833954;-1.71323827298365;-4.98642371014533]
Robot Arm 1 end: [-1.31402924147935;-5.14974679940902;1.27020785129981]
Robot Arm 1 togoal: 18.9099175689664
Mobile Platform 2 Pose: [-12.3515949830113;-1.42968677986904;3.72142475413459]
Robot Arm 2 end: [-0.682461104174836;4.05524766090719;0.637744136873261]
Robot Arm 1 togoal: 16.8862861163845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [-0.300871974345298;-1.78862697141959;-4.77144709654712]
Robot Arm 1 end: [-1.24088455327161;-4.81068971716685;1.19166171654295]
Robot Arm 1 togoal: 18.5963028353252
Mobile Platform 2 Pose: [-12.3051969880689;-1.4499228795364;3.81190504898419]
Robot Arm 2 end: [-0.639745615831712;4.02992166145768;0.618257283711916]
Robot Arm 1 togoal: 16.8898788738935

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.418221e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [-0.599030859353991;-1.56089552117067;-4.66830453538203]
Robot Arm 1 end: [-1.14615303354088;-4.88189992992022;1.23983276092343]
Robot Arm 1 togoal: 18.5953415668442
Mobile Platform 2 Pose: [-12.0689083227406;-1.58246592947236;3.9100258339804]
Robot Arm 2 end: [-0.736897776759353;3.94248544907376;0.513147886163416]
Robot Arm 1 togoal: 16.7694284094361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.299027e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [-0.479584600780478;-1.5859776321198;-4.50425726757467]
Robot Arm 1 end: [-1.06750410253388;-4.54104245698286;1.20633679127414]
Robot Arm 1 togoal: 18.2765033781166
Mobile Platform 2 Pose: [-11.7273796950525;-1.73721909980633;4.06097323825212]
Robot Arm 2 end: [-0.87300534829877;4.00189273337097;0.642946234611085]
Robot Arm 1 togoal: 16.7369653732068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.779267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.460326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [-0.49625880472032;-1.52175712677053;-4.31096323585065]
Robot Arm 1 end: [-1.06399487838104;-4.28415505416951;1.24884395086338]
Robot Arm 1 togoal: 18.0699512228307
Mobile Platform 2 Pose: [-11.5528091359232;-1.98731718518128;4.22427856825562]
Robot Arm 2 end: [-1.12860453161298;4.05692660937445;0.653647080055029]
Robot Arm 1 togoal: 16.6448533891289

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.708275e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.923339e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [-0.699527196176664;-1.40153042416603;-4.29979664245617]
Robot Arm 1 end: [-0.863271629331332;-4.47198236556733;1.12576981371903]
Robot Arm 1 togoal: 18.0998660487329
Mobile Platform 2 Pose: [-11.8767604448561;-1.88437974512035;4.230449911314]
Robot Arm 2 end: [-1.02010612869542;4.12535055597046;0.547460717849342]
Robot Arm 1 togoal: 16.7663890269443

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.384818e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [-0.990201456775516;-1.15525453525308;-4.16235129625796]
Robot Arm 1 end: [-0.664285866368387;-4.51030362468435;1.07146858753856]
Robot Arm 1 togoal: 18.0132724070598
Mobile Platform 2 Pose: [-12.0413576366368;-1.83834578366365;4.22726301876336]
Robot Arm 2 end: [-1.02271351674889;4.00686715020542;0.460538389327318]
Robot Arm 1 togoal: 16.6705902353089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.626038e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [-0.798584260662951;-1.24261518892831;-4.09620471191616]
Robot Arm 1 end: [-0.736052611029807;-4.32208716200982;1.16667624862034]
Robot Arm 1 togoal: 17.9028084853689
Mobile Platform 2 Pose: [-12.1956399807246;-1.78789321839348;4.22320756481853]
Robot Arm 2 end: [-1.00881510431062;4.02736757930426;0.398916159342131]
Robot Arm 1 togoal: 16.6993269079581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.901047e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [-0.876593720627799;-1.07633393792663;-3.92996950953999]
Robot Arm 1 end: [-0.540120425166853;-4.1402421320418;1.14265340964346]
Robot Arm 1 togoal: 17.6404005700614
Mobile Platform 2 Pose: [-12.2834219337988;-1.75313576318943;4.21773021122317]
Robot Arm 2 end: [-0.979929297419909;4.04316575743829;0.390637966354923]
Robot Arm 1 togoal: 16.7287147831965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.307893e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [-0.712976877051395;-1.07566992561641;-3.78858362192365]
Robot Arm 1 end: [-0.572695077436798;-3.81467215597644;1.23776139568478]
Robot Arm 1 togoal: 17.3984847640403
Mobile Platform 2 Pose: [-12.334049136019;-1.72967246617698;4.21426208843204]
Robot Arm 2 end: [-0.964814756328696;4.05911801145441;0.398861244672985]
Robot Arm 1 togoal: 16.7497034909178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.107696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [-0.986725638567278;-0.872742939672905;-3.70476986917145]
Robot Arm 1 end: [-0.316621916236634;-3.92650392449452;1.11696128806538]
Robot Arm 1 togoal: 17.3361889295853
Mobile Platform 2 Pose: [-12.3607759909088;-1.71686706108505;4.21260330205725]
Robot Arm 2 end: [-0.967841101560738;4.06487563469892;0.412988864371203]
Robot Arm 1 togoal: 16.7519643523642

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.317909e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [-0.818514122212185;-0.941787987762972;-3.64729198838752]
Robot Arm 1 end: [-0.359441612867017;-3.6868608198086;1.16148071338331]
Robot Arm 1 togoal: 17.169083951937
Mobile Platform 2 Pose: [-12.3655286223456;-1.71591300497124;4.2120219463604]
Robot Arm 2 end: [-0.990861780282968;4.06006472498682;0.427921954788546]
Robot Arm 1 togoal: 16.7345394661971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.740119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [-1.05579122394087;-0.719561046121417;-3.53275073997296]
Robot Arm 1 end: [-0.120295558590709;-3.76977807010127;1.05302795939795]
Robot Arm 1 togoal: 17.095208923083
Mobile Platform 2 Pose: [-12.3447547048009;-1.7307543303301;4.21147422877066]
Robot Arm 2 end: [-1.03978888222388;4.03869516893855;0.442050563948991]
Robot Arm 1 togoal: 16.6893522105279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.114927e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [-0.85131527885285;-0.78895097259711;-3.46731236716714]
Robot Arm 1 end: [-0.160255537049763;-3.51651980232304;1.10369613750101]
Robot Arm 1 togoal: 16.9145275623766
Mobile Platform 2 Pose: [-12.2921560286951;-1.76810074721783;4.20877340247575]
Robot Arm 2 end: [-1.12272541442388;3.9845918788081;0.459432915955486]
Robot Arm 1 togoal: 16.5982450505609

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.137417e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [-1.10300878727491;-0.556696395633693;-3.37480392629127]
Robot Arm 1 end: [0.080595341355695;-3.62757730293426;0.98032196070211]
Robot Arm 1 togoal: 16.8640975053977
Mobile Platform 2 Pose: [-12.2351136882395;-1.81235788291323;4.20394792472076]
Robot Arm 2 end: [-1.22920782684304;3.91229013262635;0.464984203632254]
Robot Arm 1 togoal: 16.480102591952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.093664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.931059e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [-0.883388193032433;-0.621586423487871;-3.30998063525648]
Robot Arm 1 end: [0.064289439900267;-3.37302381242225;1.01190895349831]
Robot Arm 1 togoal: 16.6677918128143
Mobile Platform 2 Pose: [-12.2391072554843;-1.82683156241844;4.19890660756544]
Robot Arm 2 end: [-1.27354064284728;3.90338284541762;0.465734125608502]
Robot Arm 1 togoal: 16.4489736313226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.591668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951114e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [-1.10578178954972;-0.380231956996891;-3.21061159881467]
Robot Arm 1 end: [0.251637997052527;-3.45957779618268;0.99294268039878]
Robot Arm 1 togoal: 16.6273762429551
Mobile Platform 2 Pose: [-12.2725307586939;-1.83314762516031;4.18602746317187]
Robot Arm 2 end: [-1.29574814089755;3.93225267561258;0.472471102948749]
Robot Arm 1 togoal: 16.461199163237

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.120061e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.765440e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [-0.874819092243768;-0.434451415384095;-3.1314543798923]
Robot Arm 1 end: [0.218416301190973;-3.18634670069433;1.0683431006273]
Robot Arm 1 togoal: 16.4244944286352
Mobile Platform 2 Pose: [-12.3932993819846;-1.78564423258087;4.16674888330629]
Robot Arm 2 end: [-1.25535244924272;3.95471420551691;0.490162915599198]
Robot Arm 1 togoal: 16.5004707338669

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.922327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998353e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [-1.13029170564359;-0.199021288848175;-3.0473552979516]
Robot Arm 1 end: [0.445975621797757;-3.30308440817963;0.963121473623975]
Robot Arm 1 togoal: 16.3878816520204
Mobile Platform 2 Pose: [-12.5245707090083;-1.71700542698518;4.15050360344285]
Robot Arm 2 end: [-1.19637874449722;3.98246675039108;0.507270749965122]
Robot Arm 1 togoal: 16.5541768333103

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.107133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999999e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [-0.906397913490497;-0.2593781338006;-2.97990132803842]
Robot Arm 1 end: [0.432911759662235;-3.04484079298045;0.999511041881393]
Robot Arm 1 togoal: 16.1854564176008
Mobile Platform 2 Pose: [-12.6336986449577;-1.65489029902295;4.140584738897]
Robot Arm 2 end: [-1.15151201898123;4.01788717997151;0.524128134636401]
Robot Arm 1 togoal: 16.6069459581404

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.655270e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [-1.12605531639235;-0.0180916630845988;-2.88147336585854]
Robot Arm 1 end: [0.61231016978179;-3.1297222196346;0.996037424677915]
Robot Arm 1 togoal: 16.1491148865391
Mobile Platform 2 Pose: [-12.7030707486951;-1.6132332444497;4.13546449258775]
Robot Arm 2 end: [-1.13337134198154;4.06561033470953;0.55938013924063]
Robot Arm 1 togoal: 16.6548254450081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.146323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988988e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [-0.896131016752777;-0.0697298810369716;-2.80043491146465]
Robot Arm 1 end: [0.576483142774638;-2.85413559594447;1.0784764691847]
Robot Arm 1 togoal: 15.9445068582463
Mobile Platform 2 Pose: [-12.752350487192;-1.58497069962146;4.1317100944926]
Robot Arm 2 end: [-1.14548186166055;4.11620705069438;0.614675478602915]
Robot Arm 1 togoal: 16.6880561210209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.095208e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003407e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [-1.16581165146044;0.167620503940769;-2.72101532794727]
Robot Arm 1 end: [0.784491345873297;-3.01610370649748;0.975676288473049]
Robot Arm 1 togoal: 15.9575039841115
Mobile Platform 2 Pose: [-12.8015964453725;-1.55938879066737;4.12693744927358]
Robot Arm 2 end: [-1.18188643461166;4.15246336126752;0.684824373925986]
Robot Arm 1 togoal: 16.6958239006569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.730682e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000774e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [-0.962456011412089;0.100631064321356;-2.6685833923917]
Robot Arm 1 end: [0.727940502398781;-2.76352546612873;1.09178920046656]
Robot Arm 1 togoal: 15.781726507642
Mobile Platform 2 Pose: [-12.8685905931625;-1.52620885057398;4.12027401429499]
Robot Arm 2 end: [-1.21599506380702;4.17987209838267;0.776720324893957]
Robot Arm 1 togoal: 16.6967502131502

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.853608e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.909524e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
helpview('optim','constraint_tolerance','CSHelpWindow');
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619
{函数或变量 'Pinv' 无法识别。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 21)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',21,0)">第 21 行</a>)
    X_d = Pinv(mobile_robot.J(4:6,:))*s_k(4:6);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 102)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',102,0)">第 102 行</a>)
    [dx1] = mobile_robot_CBLF(mobile_robot1, a1_G, obstacle_results1, D_min, V_max, dt);
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.884541e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.784973646531428;-9.99971132853111;-9.82820014455123]
Robot Arm 1 end: [-10.0380417347551;-9.82872531833671;1.51997973247882]
Robot Arm 1 togoal: 28.1904498962819
Mobile Platform 2 Pose: [-2.37373153823092;10.0086961924501;9.81851211370782]
Robot Arm 2 end: [9.77325007000646;9.82827209721203;1.48603581684879]
Robot Arm 1 togoal: 28.0025537261549

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.858615e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.739604349491108;-9.96943303111976;-9.82072865114823]
Robot Arm 1 end: [-10.1974699291175;-9.79506744008313;1.48828174146052]
Robot Arm 1 togoal: 28.2804447804395
Mobile Platform 2 Pose: [-2.33467356566013;9.98249057225859;9.81776770544381]
Robot Arm 2 end: [9.74712852114439;9.81130564162149;1.48234114273615]
Robot Arm 1 togoal: 27.9721005747438

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.887063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.724841869533536;-9.95882392993013;-9.82238721416411]
Robot Arm 1 end: [-10.3142909452569;-9.79067783517213;1.43897637236528]
Robot Arm 1 togoal: 28.3610308990983
Mobile Platform 2 Pose: [-2.28857336263872;9.95191584743157;9.81560858944277]
Robot Arm 2 end: [9.7980961586305;9.79723165048392;1.49297613638729]
Robot Arm 1 togoal: 27.998138828612

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.910071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.707446299756816;-9.94613443473812;-9.82400610972553]
Robot Arm 1 end: [-10.4386002883094;-9.77939556281872;1.35325178012974]
Robot Arm 1 togoal: 28.4427321128088
Mobile Platform 2 Pose: [-2.24040719816869;9.91985077753735;9.8120825737239]
Robot Arm 2 end: [9.88572468890356;9.78474544989015;1.49122404915435]
Robot Arm 1 togoal: 28.0513651276915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.840783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.694743598083198;-9.93646434509162;-9.82539740154756]
Robot Arm 1 end: [-10.5426144886992;-9.76882813262693;1.24444191494507]
Robot Arm 1 togoal: 28.5110866180696
Mobile Platform 2 Pose: [-2.1937537490217;9.88800589219409;9.80674340016791]
Robot Arm 2 end: [9.95837098000787;9.76419325918737;1.46066720082702]
Robot Arm 1 togoal: 28.0884949295754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.896988e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.683430442629134;-9.92762963830151;-9.82667587035461]
Robot Arm 1 end: [-10.6218391832771;-9.75262622506869;1.11624106426062]
Robot Arm 1 togoal: 28.5585420475954
Mobile Platform 2 Pose: [-2.15976270668439;9.86310720156278;9.79865379449075]
Robot Arm 2 end: [10.0608760904785;9.73789664887749;1.41088234002511]
Robot Arm 1 togoal: 28.1431241964772

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.673660205265545;-9.91981700803529;-9.82786650745219]
Robot Arm 1 end: [-10.6702137448991;-9.73028446400646;0.981651818587247]
Robot Arm 1 togoal: 28.5802861733613
Mobile Platform 2 Pose: [-2.14156873516191;9.84654986981437;9.79016022905948]
Robot Arm 2 end: [10.2241648409808;9.69299545239027;1.32957410405354]
Robot Arm 1 togoal: 28.2288426485513

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.664976470879589;-9.91281690344043;-9.82897491150644]
Robot Arm 1 end: [-10.6886690918901;-9.70222317386696;0.851335210351745]
Robot Arm 1 togoal: 28.5770375951931
Mobile Platform 2 Pose: [-2.13352730691724;9.8329441814819;9.78156069308875]
Robot Arm 2 end: [10.3467205469811;9.58243216341277;1.20799727011273]
Robot Arm 1 togoal: 28.241081019687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980047e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.656833109084103;-9.90634481105832;-9.82999922198105]
Robot Arm 1 end: [-10.6818191461944;-9.67022721129422;0.732643499736038]
Robot Arm 1 togoal: 28.5530556632971
Mobile Platform 2 Pose: [-2.15013958617385;9.83100573978276;9.77238885470981]
Robot Arm 2 end: [10.4126944650323;9.43239709335412;1.07380434452742]
Robot Arm 1 togoal: 28.1868078375475

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.648766320798012;-9.90013952529218;-9.83093799505939]
Robot Arm 1 end: [-10.6558390361124;-9.63686825400093;0.629177471239773]
Robot Arm 1 togoal: 28.5143056821234
Mobile Platform 2 Pose: [-2.18857232656511;9.84705566897588;9.76559344932607]
Robot Arm 2 end: [10.372270628809;9.30480332022962;1.06713170483138]
Robot Arm 1 togoal: 28.0697495221375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.640436171253537;-9.89398799753507;-9.83179537519992]
Robot Arm 1 end: [-10.6167964117317;-9.60472993248833;0.5410851631173]
Robot Arm 1 togoal: 28.4667870174374
Mobile Platform 2 Pose: [-2.25235804808183;9.8793841630423;9.75505585863434]
Robot Arm 2 end: [10.2734939806842;9.23969036167778;1.16477860847708]
Robot Arm 1 togoal: 27.9518717991811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980268e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.631539882607877;-9.88767584010998;-9.83258627602768]
Robot Arm 1 end: [-10.5839217542558;-9.57457291186147;0.465698167471456]
Robot Arm 1 togoal: 28.4249081163835
Mobile Platform 2 Pose: [-2.37808688395762;9.94389156424565;9.72773902346401]
Robot Arm 2 end: [10.3527802135632;9.30145243242858;1.2417653157395]
Robot Arm 1 togoal: 28.0510195970094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.624049755723741;-9.88227554986445;-9.83344189124978]
Robot Arm 1 end: [-10.5571910030627;-9.55981423057153;0.412113953237322]
Robot Arm 1 togoal: 28.3974305692536
Mobile Platform 2 Pose: [-2.46805071282185;9.9928264704549;9.70958161771963]
Robot Arm 2 end: [10.416885358742;9.40827233996571;1.30444698451874]
Robot Arm 1 togoal: 28.1705012057437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.617432180399294;-9.87748918612576;-9.83427289788956]
Robot Arm 1 end: [-10.5360306324131;-9.55296683313609;0.374411923677593]
Robot Arm 1 togoal: 28.378895861712
Mobile Platform 2 Pose: [-2.55949114751025;10.0436123075565;9.69383759346064]
Robot Arm 2 end: [10.638458178486;9.4403680854804;1.17576560259721]
Robot Arm 1 togoal: 28.3547712752762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975679e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.611308743324291;-9.87307847110079;-9.83505465661143]
Robot Arm 1 end: [-10.5192517036697;-9.54975790490078;0.347581315773475]
Robot Arm 1 togoal: 28.3656418229391
Mobile Platform 2 Pose: [-2.5461777903025;10.0352327054271;9.69506572740273]
Robot Arm 2 end: [10.7053373496572;9.43182074761631;1.06950677573694]
Robot Arm 1 togoal: 28.3991284806798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981521e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.605501272721076;-9.86892306342162;-9.83578374406352]
Robot Arm 1 end: [-10.505892035287;-9.54821757399887;0.328693151828132]
Robot Arm 1 togoal: 28.3557051420599
Mobile Platform 2 Pose: [-2.50313687304556;10.0081302886844;9.69736590393471]
Robot Arm 2 end: [10.724435416714;9.35668582963626;0.907496514926282]
Robot Arm 1 togoal: 28.3647650470613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000015e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.599917677615897;-9.86495791241877;-9.83646122170501]
Robot Arm 1 end: [-10.4952618442746;-9.54739525667926;0.315989086874684]
Robot Arm 1 togoal: 28.3479888296679
Mobile Platform 2 Pose: [-2.47464427127805;9.98773110417042;9.69839867894779]
Robot Arm 2 end: [10.6681650917151;9.22195149648857;0.742115266116627]
Robot Arm 1 togoal: 28.2358482964551

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000022e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.594503023034803;-9.86114329093687;-9.83708888821633]
Robot Arm 1 end: [-10.4868456191286;-9.54683212722049;0.308332523642406]
Robot Arm 1 togoal: 28.3418427777723
Mobile Platform 2 Pose: [-2.46061120909515;9.9766759074077;9.69868729187942]
Robot Arm 2 end: [10.587473197578;9.1304506097543;0.573994107001827]
Robot Arm 1 togoal: 28.1196552851313

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000013e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.589218891603615;-9.85745123633009;-9.83766864450077]
Robot Arm 1 end: [-10.4802206092727;-9.54632703470604;0.304914065612517]
Robot Arm 1 togoal: 28.3368523614034
Mobile Platform 2 Pose: [-2.45336888351686;9.97008855568352;9.69851169069725]
Robot Arm 2 end: [10.4907988980123;9.08529875406211;0.409606119927836]
Robot Arm 1 togoal: 28.0242156780033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000029e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.58403458404484;-9.85385943447178;-9.83820251649745]
Robot Arm 1 end: [-10.4750087226887;-9.5458213855304;0.305096443778486]
Robot Arm 1 togoal: 28.3327290829399
Mobile Platform 2 Pose: [-2.45044639767236;9.96613324097559;9.69801163103693]
Robot Arm 2 end: [10.3854957947026;9.08453495617819;0.254846735417568]
Robot Arm 1 togoal: 27.953372915962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000046e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.578923225113092;-9.85034837833736;-9.83869275513583]
Robot Arm 1 end: [-10.4708526571268;-9.54533586613511;0.308331663315341]
Robot Arm 1 togoal: 28.329252145523
Mobile Platform 2 Pose: [-2.45077602615625;9.96391497473277;9.69723609237692]
Robot Arm 2 end: [10.2817446570894;9.12689375616327;0.114948617446139]
Robot Arm 1 togoal: 27.9135130642009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000138e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.573859868663879;-9.84689993977582;-9.83914189855625]
Robot Arm 1 end: [-10.4701792521911;-9.54329053133218;0.320965044321749]
Robot Arm 1 togoal: 28.3268163990192
Mobile Platform 2 Pose: [-2.44942364764665;9.96068594875695;9.69613720142776]
Robot Arm 2 end: [10.2021935788154;9.14954871965211;0.0496876864409377]
Robot Arm 1 togoal: 27.8746946438215

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.568744357325611;-9.84344509223267;-9.83955967828979]
Robot Arm 1 end: [-10.4703292593784;-9.53918755130075;0.339347362866552]
Robot Arm 1 togoal: 28.32332136433
Mobile Platform 2 Pose: [-2.44888817427484;9.95769714027793;9.69464340807194]
Robot Arm 2 end: [10.1313653378437;9.1697005556971;0.00311090024682353]
Robot Arm 1 togoal: 27.8397786823095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.262981e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.563578085693075;-9.83999685095606;-9.83993285149266]
Robot Arm 1 end: [-10.4689997439972;-9.53418028432365;0.358263948079237]
Robot Arm 1 togoal: 28.3181232610258
Mobile Platform 2 Pose: [-2.44922897404403;9.95483468052728;9.69229882418093]
Robot Arm 2 end: [10.0610820256892;9.18012941852079;-0.0291812720708753]
Robot Arm 1 togoal: 27.7979754393898

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.124883e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.558427860876471;-9.83661029535697;-9.8402448665039]
Robot Arm 1 end: [-10.4668079959747;-9.52948408373581;0.377891202822517]
Robot Arm 1 togoal: 28.3125003460163
Mobile Platform 2 Pose: [-2.44923290615733;9.9515242604094;9.68839735063949]
Robot Arm 2 end: [9.98179011768675;9.16202072650239;-0.0369539056996825]
Robot Arm 1 togoal: 27.7287250879386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.550836e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.553305003392368;-9.83329403048955;-9.84049291258899]
Robot Arm 1 end: [-10.4639892150354;-9.52574559539454;0.398232078788745]
Robot Arm 1 togoal: 28.307069849316
Mobile Platform 2 Pose: [-2.40496575698332;9.92714906909227;9.68512368546733]
Robot Arm 2 end: [9.88695210931448;9.02388169377875;0.0908662500876297]
Robot Arm 1 togoal: 27.5580155569971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.548186532567093;-9.83003037802041;-9.8406801886397]
Robot Arm 1 end: [-10.4605013346584;-9.52313382136095;0.419200966203105]
Robot Arm 1 togoal: 28.3019227294402
Mobile Platform 2 Pose: [-2.40060337770089;9.92437223984419;9.6848654598841]
Robot Arm 2 end: [9.8668507089445;9.03270570478073;0.0855374475345373]
Robot Arm 1 togoal: 27.5498855946048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.543030359830344;-9.82678962848902;-9.84081039152489]
Robot Arm 1 end: [-10.4561168634111;-9.52147072313214;0.440695753547439]
Robot Arm 1 togoal: 28.2967770883637
Mobile Platform 2 Pose: [-2.36296687047783;9.90486628206191;9.6850112083348]
Robot Arm 2 end: [9.82928819212847;9.03383001536616;0.0902884771620622]
Robot Arm 1 togoal: 27.5233408350425

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.53778014194434;-9.82353605342773;-9.84088407669266]
Robot Arm 1 end: [-10.4504833005712;-9.52035088076928;0.462653944448298]
Robot Arm 1 togoal: 28.2911021720362
Mobile Platform 2 Pose: [-2.30775630636722;9.87613419224139;9.68569947033048]
Robot Arm 2 end: [9.81995629785389;8.99664961170243;0.139047851076283]
Robot Arm 1 togoal: 27.4884185769307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.532361100518938;-9.82022742038682;-9.84089592893575]
Robot Arm 1 end: [-10.4431455940323;-9.51923731122839;0.485099827589931]
Robot Arm 1 togoal: 28.2841990545614
Mobile Platform 2 Pose: [-2.26548328162992;9.85305518326086;9.68665879737523]
Robot Arm 2 end: [9.7511038555639;8.9521818523635;0.21949060152581]
Robot Arm 1 togoal: 27.4041096770657

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991336e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.526667979254746;-9.81680978137528;-9.8408316411068]
Robot Arm 1 end: [-10.4335309257523;-9.51754834825859;0.508195265741002]
Robot Arm 1 togoal: 28.2752480634569
Mobile Platform 2 Pose: [-2.26632136209624;9.85059050483142;9.68750183887291]
Robot Arm 2 end: [9.68508425781392;8.90669253716932;0.347946557236026]
Robot Arm 1 togoal: 27.3192536375331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985153e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.520544131835174;-9.81320812628348;-9.84066260369877]
Robot Arm 1 end: [-10.4208981939122;-9.51477718641221;0.532311516304235]
Robot Arm 1 togoal: 28.2633528902576
Mobile Platform 2 Pose: [-2.26395088481605;9.84565648041892;9.6880119994844]
Robot Arm 2 end: [9.73376449119636;8.85149008161209;0.516191833076734]
Robot Arm 1 togoal: 27.3095180979716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988619e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.513748627192006;-9.80931284062521;-9.84033525389691]
Robot Arm 1 end: [-10.4042623176981;-9.51074065581038;0.558146269388513]
Robot Arm 1 togoal: 28.2476549287597
Mobile Platform 2 Pose: [-2.26923785880209;9.84361222463695;9.68797266269027]
Robot Arm 2 end: [9.72802176403839;8.85364538639183;0.653571078027711]
Robot Arm 1 togoal: 27.3021468022729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989351e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.505911883593327;-9.80496556319365;-9.83974872637341]
Robot Arm 1 end: [-10.3823606077463;-9.50621866686771;0.586906783004054]
Robot Arm 1 togoal: 28.2277473554285
Mobile Platform 2 Pose: [-2.27730372047106;9.84236971213215;9.68745056052251]
Robot Arm 2 end: [9.68611033790115;8.88736330545792;0.785601819266924]
Robot Arm 1 togoal: 27.2913217589878

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.631038e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992784e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.496452039967155;-9.79993908037817;-9.83871115483099]
Robot Arm 1 end: [-10.321704100002;-9.45358570453236;0.622755536667416]
Robot Arm 1 togoal: 28.146411803417
Mobile Platform 2 Pose: [-2.28816107215455;9.84199012445592;9.68582260218754]
Robot Arm 2 end: [9.56106248382922;8.9664198698303;0.888576256094718]
Robot Arm 1 togoal: 27.2536274276805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.861375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.496794559498609;-9.7997106461131;-9.83825219187601]
Robot Arm 1 end: [-10.2629561116265;-9.40053210912816;0.645241542210623]
Robot Arm 1 togoal: 28.0666169919961
Mobile Platform 2 Pose: [-2.28211082617694;9.82995713886821;9.68097925350074]
Robot Arm 2 end: [9.43796078992617;9.01876470558743;0.842388992081173]
Robot Arm 1 togoal: 27.2030925355422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.206072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.097548e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.490621917058851;-9.79515038783375;-9.83867819445694]
Robot Arm 1 end: [-10.2517872363561;-9.38773760027926;0.660543473797893]
Robot Arm 1 togoal: 28.0492360137919
Mobile Platform 2 Pose: [-2.23712586011646;9.78986492580917;9.66594184700899]
Robot Arm 2 end: [9.26014926323031;9.07736366301538;0.759931418448746]
Robot Arm 1 togoal: 27.1197046325854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.716449e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.660749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.476917827554629;-9.78603564309247;-9.83924222630474]
Robot Arm 1 end: [-10.2558472960815;-9.38484981238157;0.669270810971783]
Robot Arm 1 togoal: 28.0499054284004
Mobile Platform 2 Pose: [-1.93208391365953;9.58372791896295;9.6007107672644]
Robot Arm 2 end: [8.95858561805107;9.11136795824262;0.774140335237146]
Robot Arm 1 togoal: 26.9300975347503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.772369e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.456603389381655;-9.77305643914337;-9.83959464811824]
Robot Arm 1 end: [-10.264648499942;-9.38616574457139;0.676415108849145]
Robot Arm 1 togoal: 28.056955015478
Mobile Platform 2 Pose: [-1.75434661907425;9.47705301497026;9.57411660129473]
Robot Arm 2 end: [8.94765427931737;8.97427890390365;0.619723079402216]
Robot Arm 1 togoal: 26.8300048956249

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.807628e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.431251228482095;-9.75723724877317;-9.83954452928202]
Robot Arm 1 end: [-10.272220762874;-9.38925444993308;0.686348224359041]
Robot Arm 1 togoal: 28.0642613722422
Mobile Platform 2 Pose: [-1.40676900429948;9.24081559478077;9.49231268117784]
Robot Arm 2 end: [8.617763245426;8.99767087888228;0.780245911796467]
Robot Arm 1 togoal: 26.609797513402

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.836925e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.402166401336661;-9.7394044114671;-9.83906345620725]
Robot Arm 1 end: [-10.2777355103715;-9.39577892126011;0.70090737459759]
Robot Arm 1 togoal: 28.072323546604
Mobile Platform 2 Pose: [-1.22183658523653;9.12970369615922;9.46476092150107]
Robot Arm 2 end: [8.60578249391953;8.86068347214467;0.664864270731168]
Robot Arm 1 togoal: 26.5072347036503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.874903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.369830358963542;-9.71990492111772;-9.83823155352725]
Robot Arm 1 end: [-10.2891885076305;-9.41824893122008;0.719658461103119]
Robot Arm 1 togoal: 28.0955816831572
Mobile Platform 2 Pose: [-0.878508483378009;8.89304175185866;9.38419340859808]
Robot Arm 2 end: [8.25642961058338;8.87829382272057;0.528489652555917]
Robot Arm 1 togoal: 26.2806445235451

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883003e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.339040129231614;-9.7013278643282;-9.83719278797435]
Robot Arm 1 end: [-10.2879626912215;-9.42816783966878;0.738562983066136]
Robot Arm 1 togoal: 28.1010203276003
Mobile Platform 2 Pose: [-0.752157733619307;8.81844889084761;9.36582501830774]
Robot Arm 2 end: [8.30699766064899;8.75486626772664;0.734406889048113]
Robot Arm 1 togoal: 26.2204100042222

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.899786e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.306736180843629;-9.6819748049086;-9.8359507609477]
Robot Arm 1 end: [-10.2854176904871;-9.44270114542049;0.759093863391061]
Robot Arm 1 togoal: 28.1086694752467
Mobile Platform 2 Pose: [-0.390917294265666;8.58113643722553;9.28719423957587]
Robot Arm 2 end: [8.17575441883687;8.58392436530697;0.523333978302391]
Robot Arm 1 togoal: 26.0137584943077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.915545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.273012396083;-9.66187773704295;-9.83456305929115]
Robot Arm 1 end: [-10.2828284279708;-9.46361308287028;0.779637320002184]
Robot Arm 1 togoal: 28.1207215750499
Mobile Platform 2 Pose: [-0.330181279827636;8.53384545319129;9.26484457734964]
Robot Arm 2 end: [8.0223311238418;8.64928802422278;0.723869653427867]
Robot Arm 1 togoal: 25.9467874646794

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.916464e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.239764831064898;-9.64200309100979;-9.83306182173482]
Robot Arm 1 end: [-10.2686887979054;-9.46869470503911;0.798974547004528]
Robot Arm 1 togoal: 28.1135401516623
Mobile Platform 2 Pose: [0.038284741187329;8.29660877066421;9.18719540526688]
Robot Arm 2 end: [7.89510735030489;8.49016765630179;0.54536345883878]
Robot Arm 1 togoal: 25.7502045370039

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.946617e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.200362975117818;-9.61882701207862;-9.83136370883164]
Robot Arm 1 end: [-10.273921268981;-9.51961178871997;0.817371899581088]
Robot Arm 1 togoal: 28.1521263940555
Mobile Platform 2 Pose: [0.126251247441857;8.22772214871609;9.15530982083825]
Robot Arm 2 end: [7.72354812590575;8.5425560811333;0.757905289065919]
Robot Arm 1 togoal: 25.6618950533885

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.929763e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.167892416394641;-9.59939766722559;-9.82963677584812]
Robot Arm 1 end: [-10.2486716417548;-9.50802070868922;0.838307498700072]
Robot Arm 1 togoal: 28.1253914510996
Mobile Platform 2 Pose: [0.457688810506939;8.01312974406106;9.08550944437794]
Robot Arm 2 end: [7.60942581908354;8.37968330506373;0.548284456728133]
Robot Arm 1 togoal: 25.4725501589528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.126426227448261;-9.57507665295934;-9.82756576386903]
Robot Arm 1 end: [-10.2417461645045;-9.54678479558005;0.857381088051947]
Robot Arm 1 togoal: 28.146855484996
Mobile Platform 2 Pose: [0.530942613783878;7.95726752590457;9.0598833509498]
Robot Arm 2 end: [7.45467225688243;8.44268815537825;0.758269017259693]
Robot Arm 1 togoal: 25.4043320822206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942953e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007303e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.0887779431886812;-9.55284521193722;-9.8253258220269]
Robot Arm 1 end: [-10.2130656480419;-9.54165221696288;0.885026809576634]
Robot Arm 1 togoal: 28.1220315084049
Mobile Platform 2 Pose: [0.877925223847548;7.73333954263637;8.98730199676492]
Robot Arm 2 end: [7.47986982751302;8.22080536208458;0.549293000115226]
Robot Arm 1 togoal: 25.2683163610755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.0428361042918613;-9.52605603762047;-9.82270893888105]
Robot Arm 1 end: [-10.1961442877851;-9.57511785508204;0.907584637091309]
Robot Arm 1 togoal: 28.1326458325704
Mobile Platform 2 Pose: [0.920302795779893;7.698622721173;8.96706092156053]
Robot Arm 2 end: [7.32306506380131;8.24701703071729;0.72985336149996]
Robot Arm 1 togoal: 25.1727655756691

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.955051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.617680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-0.00272507396987234;-9.49949588018272;-9.81977426161485]
Robot Arm 1 end: [-10.1659383907709;-9.58754571329481;0.938203279335138]
Robot Arm 1 togoal: 28.1189734924375
Mobile Platform 2 Pose: [1.21503550915647;7.49454003132377;8.86725341934288]
Robot Arm 2 end: [7.01031297190757;8.26920678972992;0.88683466272623]
Robot Arm 1 togoal: 24.9703189768004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002254e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-0.0548171908805258;-9.46931663877192;-9.81639831253528]
Robot Arm 1 end: [-10.1396147011276;-9.62064233995161;0.961953250800875]
Robot Arm 1 togoal: 28.1226993620806
Mobile Platform 2 Pose: [1.37129712202378;7.4018108556416;8.83644583999007]
Robot Arm 2 end: [7.018814013834;8.12028792634807;0.681409284348892]
Robot Arm 1 togoal: 24.8734753812377

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.714683e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-0.109569553450831;-9.43772818098335;-9.8125551967085]
Robot Arm 1 end: [-10.108098428681;-9.65162264334525;0.985619680812969]
Robot Arm 1 togoal: 28.1213180171224
Mobile Platform 2 Pose: [1.63848740165996;7.21301942655429;8.74784297374357]
Robot Arm 2 end: [6.74859702079014;8.14901103872744;0.886437832471805]
Robot Arm 1 togoal: 24.7043472488777

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972170e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-0.168818483776717;-9.40376038560632;-9.8082114035875]
Robot Arm 1 end: [-10.0797828568676;-9.69927397965919;0.998450309086183]
Robot Arm 1 togoal: 28.1341808674805
Mobile Platform 2 Pose: [1.75928710800958;7.13999760865165;8.72301986765304]
Robot Arm 2 end: [6.76491709246115;8.00159508373341;0.692620434341154]
Robot Arm 1 togoal: 24.613128272948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.965661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-0.225997722741915;-9.37105144673711;-9.80340595865668]
Robot Arm 1 end: [-10.0333353591456;-9.71233175509862;1.02978434608205]
Robot Arm 1 togoal: 28.1096396571304
Mobile Platform 2 Pose: [2.01183168044973;6.95830355393174;8.63652303323695]
Robot Arm 2 end: [6.49961227575128;8.03701213220899;0.897649239049831]
Robot Arm 1 togoal: 24.4532120196663

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-0.280408400170833;-9.33918255769381;-9.79803623325662]
Robot Arm 1 end: [-9.98373932517299;-9.71355976194637;1.05758791344781]
Robot Arm 1 togoal: 28.0747043914314
Mobile Platform 2 Pose: [2.13236004520392;6.88556748843269;8.61173821877559]
Robot Arm 2 end: [6.5119632163498;7.88924624868782;0.699108343466917]
Robot Arm 1 togoal: 24.3586856480262

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-0.338318804915629;-9.3051120201963;-9.79227175383487]
Robot Arm 1 end: [-9.94569208942391;-9.72564126271504;1.06076606115627]
Robot Arm 1 togoal: 28.0560754816118
Mobile Platform 2 Pose: [2.40322637728388;6.69368820291334;8.52148000832659]
Robot Arm 2 end: [6.2395917443697;7.9204813906704;0.903166293576449]
Robot Arm 1 togoal: 24.1919348499858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-0.402884363159772;-9.26788755705539;-9.78623321671148]
Robot Arm 1 end: [-9.95745820320396;-9.847793093634;0.96382878392475]
Robot Arm 1 togoal: 28.1521840784043
Mobile Platform 2 Pose: [2.52383251702366;6.62105440727107;8.49671768400263]
Robot Arm 2 end: [6.24946317574597;7.77305738275426;0.704249426468683]
Robot Arm 1 togoal: 24.0955121076512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-0.414903606622172;-9.25925085006865;-9.78045131416981]
Robot Arm 1 end: [-9.94161147809678;-9.88988370062187;0.960784444474592]
Robot Arm 1 togoal: 28.1707149968949
Mobile Platform 2 Pose: [2.81060924077734;6.42060086949898;8.40344164081402]
Robot Arm 2 end: [5.97108267833265;7.80060959005049;0.90738422175281]
Robot Arm 1 togoal: 23.9230789828326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975834e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-0.413851659574317;-9.2571686966037;-9.77470867804348]
Robot Arm 1 end: [-9.9369110358095;-9.92746763130288;0.947767793723434]
Robot Arm 1 togoal: 28.1942012301948
Mobile Platform 2 Pose: [2.93147052819032;6.34793712516951;8.37866413359024]
Robot Arm 2 end: [5.97881938804136;7.65368925460277;0.709127977786522]
Robot Arm 1 togoal: 23.8250823611205

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975334e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-0.396309098740765;-9.26415431496036;-9.76945365492121]
Robot Arm 1 end: [-9.93867847544544;-9.95580476976457;0.940921830417821]
Robot Arm 1 togoal: 28.2156257070258
Mobile Platform 2 Pose: [3.23190893643403;6.14035340377392;8.28301051241736]
Robot Arm 2 end: [5.69564905198239;7.67813993930755;0.911274884678147]
Robot Arm 1 togoal: 23.6482937141983

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975354e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-0.365253905071031;-9.27857635852806;-9.76468304534684]
Robot Arm 1 end: [-9.9477885042833;-9.97691241709984;0.936077049707609]
Robot Arm 1 togoal: 28.2370931717462
Mobile Platform 2 Pose: [3.35316147903959;6.06753942562764;8.25817550136699]
Robot Arm 2 end: [5.70170236326555;7.53280629070838;0.713634376091677]
Robot Arm 1 togoal: 23.5498321179314

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976359e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-0.322784324084942;-9.29926144241131;-9.76042418376439]
Robot Arm 1 end: [-9.96447402765997;-9.99224893182632;0.931055744462314]
Robot Arm 1 togoal: 28.2598350097424
Mobile Platform 2 Pose: [3.6750812477508;5.84716857733684;8.1575539942326]
Robot Arm 2 end: [5.40635223870735;7.54860085455298;0.916082568733707]
Robot Arm 1 togoal: 23.3596876339173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-0.35021032464321;-9.28542072286809;-9.75676983823239]
Robot Arm 1 end: [-9.94716528415546;-9.99525592647818;0.935392218512504]
Robot Arm 1 togoal: 28.2496477661672
Mobile Platform 2 Pose: [3.804364030303;5.76968621049956;8.13112994450646]
Robot Arm 2 end: [5.40692382190135;7.40519260120307;0.717539932160485]
Robot Arm 1 togoal: 23.2585377704502

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-0.37412597607583;-9.27359463014866;-9.75346924701837]
Robot Arm 1 end: [-9.93395705686117;-9.99635836824222;0.936302853038213]
Robot Arm 1 togoal: 28.2410846585187
Mobile Platform 2 Pose: [4.13967083697966;5.54225718432189;8.02732224704509]
Robot Arm 2 end: [5.1059523288748;7.4158137513842;0.9145460013442]
Robot Arm 1 togoal: 23.0622668191491

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984762e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005062e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-0.393838930890671;-9.26399172004723;-9.75068677957585]
Robot Arm 1 end: [-9.924724573704;-10.0041064207299;0.927988390476643]
Robot Arm 1 togoal: 28.2402299175994
Mobile Platform 2 Pose: [4.26634874653604;5.4663696622073;8.0014945608695]
Robot Arm 2 end: [5.11672816223426;7.27402466717099;0.739044752324152]
Robot Arm 1 togoal: 22.9677785722147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988607e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003441e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-0.407669090426012;-9.25731221085264;-9.748716410251]
Robot Arm 1 end: [-9.92153409662717;-10.0129242174405;0.912569957319318]
Robot Arm 1 togoal: 28.2445542293188
Mobile Platform 2 Pose: [4.60622216826469;5.23144735154161;7.88931055924302]
Robot Arm 2 end: [4.73225993121954;7.28859746235981;0.898628387030861]
Robot Arm 1 togoal: 22.7227293985815

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990966e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003049e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-0.415854796903567;-9.25329352627539;-9.74757989997415]
Robot Arm 1 end: [-9.92360139818062;-10.0201887709239;0.892925688480175]
Robot Arm 1 togoal: 28.2515898391188
Mobile Platform 2 Pose: [4.76479949352484;5.13911894169335;7.86128040043408]
Robot Arm 2 end: [4.99310586654437;7.12052372702365;0.9182293116868]
Robot Arm 1 togoal: 22.7655197330453

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990587e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-0.419693581995995;-9.25122155847792;-9.74711144233141]
Robot Arm 1 end: [-9.92678883270121;-10.026802481361;0.874469332358271]
Robot Arm 1 togoal: 28.2589407571252
Mobile Platform 2 Pose: [4.79780527393056;5.10466059983464;7.82795735400105]
Robot Arm 2 end: [4.57979994131606;7.18505198104415;0.581537133182266]
Robot Arm 1 togoal: 22.5561337200797

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976209e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-0.420165651391428;-9.25056044823988;-9.74717773400428]
Robot Arm 1 end: [-9.93492612148533;-10.035809648929;0.849538633238791]
Robot Arm 1 togoal: 28.2716431804777
Mobile Platform 2 Pose: [4.89131106192613;5.05232229322667;7.81283609290608]
Robot Arm 2 end: [4.71255604549139;7.09869463811477;0.841600393503611]
Robot Arm 1 togoal: 22.567371303378

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990898e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.934804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-0.41744484170664;-9.25113837869254;-9.7477632672269]
Robot Arm 1 end: [-9.95159478636572;-10.0392961407778;0.815683452411362]
Robot Arm 1 togoal: 28.2866929097311
Mobile Platform 2 Pose: [5.25074527258811;4.82468085602113;7.7102943740083]
Robot Arm 2 end: [4.59354523525262;6.91960795430932;0.634355917771632]
Robot Arm 1 togoal: 22.3613692243888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.949110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-0.412936458542874;-9.2522317289621;-9.74866949607862]
Robot Arm 1 end: [-9.97725436317202;-10.0355985564661;0.761292144958116]
Robot Arm 1 togoal: 28.3035873597633
Mobile Platform 2 Pose: [5.32021694994615;4.77266296477415;7.67835691473198]
Robot Arm 2 end: [4.380127874037;7.00866922905155;0.873016936242054]
Robot Arm 1 togoal: 22.2823425309786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.889807e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-0.408997510196711;-9.25274553331515;-9.74956376938129]
Robot Arm 1 end: [-10.0031996613104;-10.0246935248178;0.671258872943315]
Robot Arm 1 togoal: 28.3167513573881
Mobile Platform 2 Pose: [5.58407265385137;4.60970791213743;7.61963187892757]
Robot Arm 2 end: [4.31908097766491;6.85719636453832;0.688270553065509]
Robot Arm 1 togoal: 22.1335624708116

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994690e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.892955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-0.407943346921683;-9.25186169731656;-9.75008994147294]
Robot Arm 1 end: [-10.0076434611672;-10.0115762513373;0.526205696666478]
Robot Arm 1 togoal: 28.3153420717147
Mobile Platform 2 Pose: [5.73809621532038;4.49727872603126;7.56173891838274]
Robot Arm 2 end: [4.01657127466135;6.96834722954613;0.913860161994348]
Robot Arm 1 togoal: 22.0172136471217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.892131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-0.432842569496287;-9.23838333663741;-9.75033152438237]
Robot Arm 1 end: [-9.98325013466951;-10.0299117528484;0.460615614684474]
Robot Arm 1 togoal: 28.3134608590382
Mobile Platform 2 Pose: [5.92895111401133;4.38195384979531;7.52988905084845]
Robot Arm 2 end: [3.99029530510381;6.83412325151202;0.74155048525437]
Robot Arm 1 togoal: 21.9025936801363

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000063e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-0.457734309983135;-9.22470178133937;-9.75044164910955]
Robot Arm 1 end: [-9.95855744834145;-10.0564651615818;0.433443700341597]
Robot Arm 1 togoal: 28.3158716782906
Mobile Platform 2 Pose: [6.31687320838092;4.11037780667693;7.42783421255157]
Robot Arm 2 end: [3.57592144102774;6.79942329291423;0.645323563949578]
Robot Arm 1 togoal: 21.6169626721267

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.901289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-0.486053018555566;-9.20873546471417;-9.75023200505361]
Robot Arm 1 end: [-9.91699977107784;-10.084452029226;0.394248237300607]
Robot Arm 1 togoal: 28.3079787821409
Mobile Platform 2 Pose: [6.46733764002168;4.02127298311674;7.40437687935621]
Robot Arm 2 end: [3.66315625366804;6.71905484492767;0.872453285441457]
Robot Arm 1 togoal: 21.6016028701764

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999124e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994449e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-0.521071451716696;-9.18892889740308;-9.7493552596297]
Robot Arm 1 end: [-9.84271780704514;-10.1031773670851;0.315449577947211]
Robot Arm 1 togoal: 28.2723282034328
Mobile Platform 2 Pose: [6.77953991918294;3.81037457589654;7.32350236291782]
Robot Arm 2 end: [3.54450124995174;6.5433030753921;0.674295620933251]
Robot Arm 1 togoal: 21.3974601076799

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000059e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994648e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-0.565229357791714;-9.16457310334522;-9.74764105552738]
Robot Arm 1 end: [-9.74017764889294;-10.1039890659287;0.223305120504552]
Robot Arm 1 togoal: 28.205123682996
Mobile Platform 2 Pose: [6.84384316826614;3.75929379155754;7.29606334901938]
Robot Arm 2 end: [3.363853663857;6.60391907518934;0.817127908075184]
Robot Arm 1 togoal: 21.3255281136532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000642e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.798975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-0.613008128024864;-9.138774565604;-9.74521613640136]
Robot Arm 1 end: [-9.68685978035712;-10.1332669377633;0.251433721380789]
Robot Arm 1 togoal: 28.1874561486928
Mobile Platform 2 Pose: [7.19294095483253;3.5336024622773;7.21305097229626]
Robot Arm 2 end: [3.31313485848445;6.46400005561392;0.80864753116104]
Robot Arm 1 togoal: 21.1851434649688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000614e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.928695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-0.673251106167785;-9.10559024450899;-9.74070750566301]
Robot Arm 1 end: [-9.61999968244003;-10.1594042082067;0.256573908407347]
Robot Arm 1 togoal: 28.1592744741392
Mobile Platform 2 Pose: [7.27198338882142;3.46055753473882;7.16716313393015]
Robot Arm 2 end: [2.90148008878158;6.56721840681444;0.606054898307144]
Robot Arm 1 togoal: 21.0180407926008

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000394e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-0.737636114750972;-9.07020632575514;-9.73495080479171]
Robot Arm 1 end: [-9.54978020537352;-10.1889243096513;0.252771466705234]
Robot Arm 1 togoal: 28.1317431822471
Mobile Platform 2 Pose: [7.38923925204862;3.39317029327148;7.15008785167109]
Robot Arm 2 end: [2.9954835809722;6.47152234772718;0.803886473436031]
Robot Arm 1 togoal: 20.9930439375755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000338e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-0.798762353478935;-9.03709801585282;-9.72900012040403]
Robot Arm 1 end: [-9.47913525060096;-10.2166101706375;0.250225590800205]
Robot Arm 1 togoal: 28.1027205889716
Mobile Platform 2 Pose: [7.70255934816906;3.18885271497695;7.07949207256839]
Robot Arm 2 end: [2.88792431175363;6.32201994753169;0.601035680859368]
Robot Arm 1 togoal: 20.8171386742176

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000277e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-0.856395847929295;-9.00630786643805;-9.72292505621357]
Robot Arm 1 end: [-9.40851860093806;-10.2415729404719;0.24879102500266]
Robot Arm 1 togoal: 28.0718789015532
Mobile Platform 2 Pose: [7.79446941179122;3.11831714250335;7.04586677552395]
Robot Arm 2 end: [2.71809910520659;6.36363496570354;0.800728274187054]
Robot Arm 1 togoal: 20.7373427781782

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000220e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-0.910498101969669;-8.97779302134807;-9.71675763627342]
Robot Arm 1 end: [-9.3388960446689;-10.2627128699234;0.248218872201712]
Robot Arm 1 togoal: 28.0390870906089
Mobile Platform 2 Pose: [8.13208428216297;2.89926178237085;6.97123544281002]
Robot Arm 2 end: [2.61474168824339;6.220763904284;0.623650594024565]
Robot Arm 1 togoal: 20.5681823566409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000238e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-0.961184939584537;-8.95142352811574;-9.71050305882342]
Robot Arm 1 end: [-9.28397212834625;-10.2717505166914;0.24795606851202]
Robot Arm 1 togoal: 28.0077862550633
Mobile Platform 2 Pose: [8.25765404348648;2.79468589062898;6.92025422763379]
Robot Arm 2 end: [2.38160021970473;6.22895667263638;0.788076198381666]
Robot Arm 1 togoal: 20.4259696460628

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-1.00953935185388;-8.92623101816655;-9.70399636541031]
Robot Arm 1 end: [-9.16747893291985;-10.1967201057927;0.12873280451884]
Robot Arm 1 togoal: 27.8789913666056
Mobile Platform 2 Pose: [8.55604459041025;2.60628579809206;6.85776938229729]
Robot Arm 2 end: [2.38518177509959;6.16593063592233;0.973430990090667]
Robot Arm 1 togoal: 20.3722819720405

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-1.0542144485642;-8.90380730447712;-9.69876738682251]
Robot Arm 1 end: [-9.10997722863789;-10.1898048889713;0.115085891933207]
Robot Arm 1 togoal: 27.835158444188
Mobile Platform 2 Pose: [8.62890360363749;2.54418484141697;6.82530406581549]
Robot Arm 2 end: [2.134022347624;6.13211849715371;0.785891746404193]
Robot Arm 1 togoal: 20.1995081544911

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-1.09766856227394;-8.88235233755235;-9.69408523249738]
Robot Arm 1 end: [-9.07457047972196;-10.2267196336612;0.147668978217238]
Robot Arm 1 togoal: 27.8360461312307
Mobile Platform 2 Pose: [8.97323455781687;2.30660594564523;6.74748203607586]
Robot Arm 2 end: [1.83322762281238;6.17345828766887;0.960129518399888]
Robot Arm 1 togoal: 20.0479576180065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-1.13769812319855;-8.8628262822006;-9.68945108618583]
Robot Arm 1 end: [-9.02994204454755;-10.2461897275244;0.166120238270504]
Robot Arm 1 togoal: 27.8187453324282
Mobile Platform 2 Pose: [9.14446486890345;2.19902899818584;6.72008994332474]
Robot Arm 2 end: [1.8071781756061;6.03803552026009;0.795085630818563]
Robot Arm 1 togoal: 19.9287478656091

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-1.17351377379465;-8.845640111753;-9.68492904087491]
Robot Arm 1 end: [-8.98166120015582;-10.2590966169766;0.179023233727567]
Robot Arm 1 togoal: 27.7945203105703
Mobile Platform 2 Pose: [9.46673203880129;1.96223453257425;6.6399127579132]
Robot Arm 2 end: [1.4090434167514;6.04041247442894;0.595621386299529]
Robot Arm 1 togoal: 19.7057714797931

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-1.20428840283222;-8.83119419792252;-9.68064298056673]
Robot Arm 1 end: [-8.92986394641642;-10.2559559436981;0.172808915532994]
Robot Arm 1 togoal: 27.7571814027129
Mobile Platform 2 Pose: [9.63644145327503;1.85315923151327;6.61619202866806]
Robot Arm 2 end: [1.41981288781519;5.95992960996721;0.795969648638834]
Robot Arm 1 togoal: 19.6381554265723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-1.22910216184932;-8.81992386955099;-9.67688516205127]
Robot Arm 1 end: [-8.88334683941726;-10.2547975286366;0.170215041723392]
Robot Arm 1 togoal: 27.724758444165
Mobile Platform 2 Pose: [9.95963253165515;1.63551505005529;6.55426412277222]
Robot Arm 2 end: [1.32674674884014;5.8196404211078;0.642262146180707]
Robot Arm 1 togoal: 19.4763547713951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-1.24835907872065;-8.81151691915078;-9.67362667381765]
Robot Arm 1 end: [-8.84115183639624;-10.2554576849264;0.170677035962886]
Robot Arm 1 togoal: 27.6964969022291
Mobile Platform 2 Pose: [10.0764971133275;1.52875357646642;6.51079875947026]
Robot Arm 2 end: [1.06741665174104;5.84932871603148;0.764445259674371]
Robot Arm 1 togoal: 19.345834362135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-1.26258823918573;-8.80560977367743;-9.67085686601205]
Robot Arm 1 end: [-8.80308861473843;-10.2577374477812;0.173888314601563]
Robot Arm 1 togoal: 27.6721299839727
Mobile Platform 2 Pose: [10.3420034266012;1.35920702363688;6.46431892580128]
Robot Arm 2 end: [1.08970698786436;5.78002643526628;0.952540077728746]
Robot Arm 1 togoal: 19.2954394108457

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-1.27242723561983;-8.80179954802508;-9.6685597421788]
Robot Arm 1 end: [-8.76959828667702;-10.2615582747767;0.17974182743119]
Robot Arm 1 togoal: 27.6518993389403
Mobile Platform 2 Pose: [10.4599906745217;1.26468884612866;6.42552169398815]
Robot Arm 2 end: [0.808136445242299;5.76065879803582;0.763993457181988]
Robot Arm 1 togoal: 19.1255650393119

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-1.27857207560287;-8.79967850059439;-9.66669738028443]
Robot Arm 1 end: [-8.74147943213067;-10.2669803659595;0.188243606669375]
Robot Arm 1 togoal: 27.6363851482629
Mobile Platform 2 Pose: [10.7365150107598;1.0665301713281;6.37437720836679]
Robot Arm 2 end: [0.562626866522518;5.82150472405305;0.961517689907292]
Robot Arm 1 togoal: 19.0316084057529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-1.28157060431723;-8.79894423535194;-9.66520749649454]
Robot Arm 1 end: [-8.71890621719832;-10.2717519797705;0.200581140608778]
Robot Arm 1 togoal: 27.6239910994381
Mobile Platform 2 Pose: [10.9589365656707;0.920241755557326;6.3438791562024]
Robot Arm 2 end: [0.500042031095785;5.67924809325099;0.802972754711279]
Robot Arm 1 togoal: 18.8839949838341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-1.28208491784427;-8.79926020395122;-9.66400828203356]
Robot Arm 1 end: [-8.70238066423257;-10.2735630038947;0.216718347680297]
Robot Arm 1 togoal: 27.6133581806221
Mobile Platform 2 Pose: [11.2232334698441;0.710191943677574;6.28923953214451]
Robot Arm 2 end: [0.127802128588466;5.71521869992386;0.606522388606509]
Robot Arm 1 togoal: 18.7183531338635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-1.28081823528604;-8.80027707643716;-9.66302310622627]
Robot Arm 1 end: [-8.69620093175569;-10.2778463773774;0.232934745037724]
Robot Arm 1 togoal: 27.6115572738326
Mobile Platform 2 Pose: [11.3550711913512;0.629394392311417;6.27296240816362]
Robot Arm 2 end: [0.213739615054027;5.60550391843324;0.803070082336728]
Robot Arm 1 togoal: 18.6646096353242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-1.27770779065539;-8.80212979805437;-9.66211162341776]
Robot Arm 1 end: [-8.69687535621345;-10.2868158494617;0.246933793259088]
Robot Arm 1 togoal: 27.6179523153856
Mobile Platform 2 Pose: [11.6911015286433;0.397575700066342;6.20825676067103]
Robot Arm 2 end: [0.114011766041106;5.47291234688738;0.652977891434032]
Robot Arm 1 togoal: 18.5056130100853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-1.27237692139824;-8.80511834160373;-9.66115249419932]
Robot Arm 1 end: [-8.6924111759944;-10.2975677218859;0.258767895993862]
Robot Arm 1 togoal: 27.622286670031
Mobile Platform 2 Pose: [11.7876905849824;0.292286803167795;6.16616050894395]
Robot Arm 2 end: [-0.164534084738844;5.50588077856558;0.606173828736807]
Robot Arm 1 togoal: 18.3849295732396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-1.26497619923615;-8.80911394039203;-9.66019089689341]
Robot Arm 1 end: [-8.68160454755525;-10.307875240815;0.271455076765797]
Robot Arm 1 togoal: 27.6219750883771
Mobile Platform 2 Pose: [11.9708316163368;0.176145700708349;6.13560462344912]
Robot Arm 2 end: [-0.0989873673672088;5.41447889150791;0.799961422378136]
Robot Arm 1 togoal: 18.3345604604723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-1.26398410383875;-8.80982133201586;-9.65934539515946]
Robot Arm 1 end: [-8.66824367786026;-10.3125140334621;0.285401827544074]
Robot Arm 1 togoal: 27.6157247092909
Mobile Platform 2 Pose: [12.1866655036467;0.00756119329593877;6.0677152075378]
Robot Arm 2 end: [-0.172692829028583;5.28535627076101;0.605268482051728]
Robot Arm 1 togoal: 18.1949621260854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-1.26295341754442;-8.81043955111094;-9.65865170801301]
Robot Arm 1 end: [-8.66674066909088;-10.3217039837746;0.307721491964754]
Robot Arm 1 togoal: 27.6204798066455
Mobile Platform 2 Pose: [12.2426383896611;-0.0482149032714587;6.03450301355591]
Robot Arm 2 end: [-0.338403305146682;5.28939558060374;0.803218360729854]
Robot Arm 1 togoal: 18.1004772974598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-1.26192314174695;-8.81098359212527;-9.65803670681824]
Robot Arm 1 end: [-8.67497318766302;-10.3331908615694;0.335435753551213]
Robot Arm 1 togoal: 27.6332921809074
Mobile Platform 2 Pose: [12.5680131302811;-0.276133921332125;5.9435746681205]
Robot Arm 2 end: [-0.367810870643112;5.23031677123765;0.982375864778284]
Robot Arm 1 togoal: 18.0286364524605

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-1.26093134950607;-8.81146373599393;-9.65746293093552]
Robot Arm 1 end: [-8.69043718989553;-10.3448760292713;0.366476415685375]
Robot Arm 1 togoal: 27.6510279682797
Mobile Platform 2 Pose: [12.6490825403097;-0.348461740897575;5.89085860326718]
Robot Arm 2 end: [-0.345532401933743;5.09194573289217;0.806627368715215]
Robot Arm 1 togoal: 17.930027854496

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-1.26002023702653;-8.81187561328224;-9.65691976746986]
Robot Arm 1 end: [-8.71073281821776;-10.3547151145126;0.397847477676379]
Robot Arm 1 togoal: 27.6706960804404
Mobile Platform 2 Pose: [12.6632908736022;-0.384094254738747;5.83698252152699]
Robot Arm 2 end: [-0.555383538632056;5.07435305539306;0.927028907371225]
Robot Arm 1 togoal: 17.7985870731319

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-1.25921914778523;-8.81221153253873;-9.65640913129786]
Robot Arm 1 end: [-8.73344968099275;-10.3617032063097;0.427573236725797]
Robot Arm 1 togoal: 27.6900104098712
Mobile Platform 2 Pose: [12.845471251939;-0.497568351639112;5.77410877287913]
Robot Arm 2 end: [-0.489516346692845;5.09664858109586;1.0979961788214]
Robot Arm 1 togoal: 17.8476057199182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-1.2585340057147;-8.81246874724758;-9.65593904571661]
Robot Arm 1 end: [-8.75604364979944;-10.3655270301601;0.454276114729096]
Robot Arm 1 togoal: 27.7070706581691
Mobile Platform 2 Pose: [12.9170560642379;-0.558141261182873;5.70081018857625]
Robot Arm 2 end: [-0.434945826877416;4.94966403575528;0.935975905954488]
Robot Arm 1 togoal: 17.757391962429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-1.25794135520809;-8.81265371265279;-9.65552019451249]
Robot Arm 1 end: [-8.77582146822358;-10.3663423719106;0.476938316818193]
Robot Arm 1 togoal: 27.7201991012831
Mobile Platform 2 Pose: [12.8730240171554;-0.570968953908247;5.59449892025199]
Robot Arm 2 end: [-0.596438608933468;4.78623178502681;0.851664091195134]
Robot Arm 1 togoal: 17.5358954089864

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-1.25738690508793;-8.81278227376077;-9.65516343118062]
Robot Arm 1 end: [-8.79000422575216;-10.3645766548172;0.494784435963595]
Robot Arm 1 togoal: 27.7278444801896
Mobile Platform 2 Pose: [12.9411607734754;-0.606875192719025;5.56203939498177]
Robot Arm 2 end: [-0.462151589132589;4.85456243079651;1.03114641635542]
Robot Arm 1 togoal: 17.6597999380663

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-1.25678972510016;-8.81287607136918;-9.65487659284788]
Robot Arm 1 end: [-8.79588598296791;-10.3606680795993;0.507226135980197]
Robot Arm 1 togoal: 27.7285028802443
Mobile Platform 2 Pose: [12.9410905283822;-0.613112008446395;5.48237419088965]
Robot Arm 2 end: [-0.308611030300323;4.79224300025924;0.90479351718259]
Robot Arm 1 togoal: 17.6950044806494

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-1.25605436034123;-8.81295573219875;-9.65465893122047]
Robot Arm 1 end: [-8.79113040741019;-10.3546977039655;0.513808738011775]
Robot Arm 1 togoal: 27.7206553826987
Mobile Platform 2 Pose: [12.750501457969;-0.538640351599566;5.39701631731521]
Robot Arm 2 end: [-0.402456461605825;4.59497250853974;0.723165138336529]
Robot Arm 1 togoal: 17.4860688824815

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-1.25509057277398;-8.81303382874246;-9.65449138541377]
Robot Arm 1 end: [-8.77427779909788;-10.345949680031;0.514099314413557]
Robot Arm 1 togoal: 27.7027981303458
Mobile Platform 2 Pose: [12.7517774838327;-0.546935867246393;5.34029671022355]
Robot Arm 2 end: [-0.326032369477028;4.61869773510742;0.90389066859405]
Robot Arm 1 togoal: 17.5406042402692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.735559e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-1.25383003962007;-8.81311660938423;-9.65432099234198]
Robot Arm 1 end: [-8.74226981805138;-10.3312095772509;0.523677620786213]
Robot Arm 1 togoal: 27.6699369846557
Mobile Platform 2 Pose: [12.6035261771597;-0.497479609778488;5.21401119974083]
Robot Arm 2 end: [-0.151276673218965;4.52168013255462;0.72402163304499]
Robot Arm 1 togoal: 17.5645016882662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.091903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-1.25210834445577;-8.81308054490362;-9.65434627368109]
Robot Arm 1 end: [-8.71913500363057;-10.3173413578215;0.542937015087189]
Robot Arm 1 togoal: 27.6433894871242
Mobile Platform 2 Pose: [12.3799274306035;-0.412625284398771;5.14895811474338]
Robot Arm 2 end: [-0.251763408142787;4.38050045017683;0.538261791830813]
Robot Arm 1 togoal: 17.4009391767846

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.270473e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-1.24926138281543;-8.81253763431362;-9.65489914908291]
Robot Arm 1 end: [-8.69441054782432;-10.2967803804919;0.571516498746143]
Robot Arm 1 togoal: 27.6105378155457
Mobile Platform 2 Pose: [12.3625910963394;-0.408863480079697;5.10896584453991]
Robot Arm 2 end: [-0.0921245128712838;4.43089933798309;0.854752818919065]
Robot Arm 1 togoal: 17.5174409568791

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.165666e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-1.27472551123691;-8.7959788794917;-9.6564621083391]
Robot Arm 1 end: [-8.70997853585863;-10.2806050653959;0.630007766789508]
Robot Arm 1 togoal: 27.607247341785
Mobile Platform 2 Pose: [12.2185707641828;-0.349608282476729;5.05192083870387]
Robot Arm 2 end: [0.0860434760767462;4.42676158602961;0.696479248009256]
Robot Arm 1 togoal: 17.6221325988631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.107580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-1.30032179986135;-8.77917547068068;-9.65830418590081]
Robot Arm 1 end: [-8.70262271581192;-10.2519275252058;0.682778701676395]
Robot Arm 1 togoal: 27.57954459162
Mobile Platform 2 Pose: [12.0367863247245;-0.274989628151495;5.00829469078263]
Robot Arm 2 end: [0.00195237802196152;4.29013923313667;0.521807248622401]
Robot Arm 1 togoal: 17.4712769089456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989043e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.086297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-1.29847872454079;-8.77687200307594;-9.65983905954596]
Robot Arm 1 end: [-8.73165871138145;-10.2282506494691;0.719580959979872]
Robot Arm 1 togoal: 27.5807801719808
Mobile Platform 2 Pose: [11.9030700651137;-0.222381427961329;4.9395505868911]
Robot Arm 2 end: [-0.0433231428189144;4.2191689236919;0.831357138715035]
Robot Arm 1 togoal: 17.3722706113776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-1.29661482187773;-8.77412656157471;-9.66133468886999]
Robot Arm 1 end: [-8.78612202809188;-10.1948818093908;0.750667519487244]
Robot Arm 1 togoal: 27.5924816622237
Mobile Platform 2 Pose: [11.8502642119625;-0.212112158395418;4.90758691329254]
Robot Arm 2 end: [0.135971135782058;4.23111588950066;0.709637203909804]
Robot Arm 1 togoal: 17.4905940411607

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989486e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-1.2950102582768;-8.77085687956411;-9.66279566922928]
Robot Arm 1 end: [-8.84909053184935;-10.155876712846;0.769913513366764]
Robot Arm 1 togoal: 27.6063708381182
Mobile Platform 2 Pose: [11.689412070367;-0.156283806837007;4.86430387031793]
Robot Arm 2 end: [0.00638661488187126;4.11045370495867;0.529208991171857]
Robot Arm 1 togoal: 17.3267517513082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990600e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-1.29415079209342;-8.76681305063554;-9.6641973592222]
Robot Arm 1 end: [-8.9142783809228;-10.1076160683564;0.77809592835881]
Robot Arm 1 togoal: 27.6155401374503
Mobile Platform 2 Pose: [11.575704901958;-0.135354031331331;4.78931635263713]
Robot Arm 2 end: [-0.0646808816518832;4.01026663966032;0.714136585581415]
Robot Arm 1 togoal: 17.1944516012381

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-1.29474559246294;-8.76180758426334;-9.66558609628682]
Robot Arm 1 end: [-8.97269901403914;-10.0526306966921;0.774436448990744]
Robot Arm 1 togoal: 27.6157321625
Mobile Platform 2 Pose: [11.5264441190424;-0.159078584736983;4.6862108900443]
Robot Arm 2 end: [0.0766243686230846;3.99669250481603;0.875949473990533]
Robot Arm 1 togoal: 17.2586746281923

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985073e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-1.33435342757691;-8.73701208955838;-9.66697505368279]
Robot Arm 1 end: [-8.98855369442225;-10.0207729557088;0.763364210232976]
Robot Arm 1 togoal: 27.6038137426685
Mobile Platform 2 Pose: [11.432051612452;-0.135967624027135;4.65260265016448]
Robot Arm 2 end: [0.0200079636965967;3.8862116785928;0.710198178932039]
Robot Arm 1 togoal: 17.143069560854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-1.34274299668022;-8.72631243334519;-9.66759393078445]
Robot Arm 1 end: [-9.02313065661112;-9.941352401254;0.738148944602835]
Robot Arm 1 togoal: 27.5708159354132
Mobile Platform 2 Pose: [11.2555969695841;-0.103741408126567;4.56781288459742]
Robot Arm 2 end: [-0.139982567790694;3.78973518482511;0.833573377640652]
Robot Arm 1 togoal: 16.9661219962667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-1.3884974755714;-8.6965311311202;-9.66821464264204]
Robot Arm 1 end: [-9.03816421049011;-9.91570583213709;0.689581400557171]
Robot Arm 1 togoal: 27.5640761170279
Mobile Platform 2 Pose: [11.2951542002844;-0.148828007547732;4.51424182776189]
Robot Arm 2 end: [-0.00536523747729173;3.82804839354797;0.997111167118776]
Robot Arm 1 togoal: 17.0699181579687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-1.44344108479028;-8.6607486187467;-9.66685779504082]
Robot Arm 1 end: [-9.0635015585158;-9.82802225960583;0.602267597023546]
Robot Arm 1 togoal: 27.5211123980706
Mobile Platform 2 Pose: [11.2416099822557;-0.144757205634368;4.4735073774093]
Robot Arm 2 end: [0.141940299391511;3.79058335716937;0.858295340647972]
Robot Arm 1 togoal: 17.1312089425407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-1.50231609696317;-8.62403381553175;-9.66543155398763]
Robot Arm 1 end: [-9.04849387937039;-9.72601262061775;0.523037411669482]
Robot Arm 1 togoal: 27.4400550711708
Mobile Platform 2 Pose: [11.1164711969218;-0.108524921517066;4.43759101643171]
Robot Arm 2 end: [-0.00136034556054812;3.65889047290027;0.687243696584282]
Robot Arm 1 togoal: 16.9479566161676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-1.55789489271476;-8.59052314274542;-9.66359142863308]
Robot Arm 1 end: [-8.99582432745785;-9.617663923993;0.445705668703863]
Robot Arm 1 togoal: 27.3285626700947
Mobile Platform 2 Pose: [11.0059805859318;-0.0987532456935881;4.37079782189872]
Robot Arm 2 end: [-0.0820929368069987;3.58650181638407;0.875927854944823]
Robot Arm 1 togoal: 16.8337170640672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990318e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-1.60465054014426;-8.56307755203341;-9.66152708636485]
Robot Arm 1 end: [-8.94699666477671;-9.53148998639372;0.320414863773595]
Robot Arm 1 togoal: 27.2380099928939
Mobile Platform 2 Pose: [11.0225677351941;-0.135512703539865;4.30132951049225]
Robot Arm 2 end: [0.0378306598657162;3.62242299497667;1.02215179056561]
Robot Arm 1 togoal: 16.9285942164807

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964043e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-1.63726718835599;-8.54572240414714;-9.65803603872634]
Robot Arm 1 end: [-8.83241843916924;-9.39587569834597;0.219157921556316]
Robot Arm 1 togoal: 27.065749041378
Mobile Platform 2 Pose: [10.9007367129102;-0.113919058037863;4.22401182514608]
Robot Arm 2 end: [0.151004626356523;3.51896685738035;0.862895585413261]
Robot Arm 1 togoal: 16.9185833105422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.871743e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-1.64823699007767;-8.54055467999703;-9.65618547263822]
Robot Arm 1 end: [-8.66840455039597;-9.31627356842249;0.16013249519883]
Robot Arm 1 togoal: 26.8975782068387
Mobile Platform 2 Pose: [10.6962304812906;-0.0544073796823943;4.16057965910015]
Robot Arm 2 end: [0.0455248286412888;3.36609279142131;0.705232640112676]
Robot Arm 1 togoal: 16.7400860803399

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-1.70074463998257;-8.50921746842439;-9.6566528169492]
Robot Arm 1 end: [-8.52412786253117;-9.37774779662986;0.152885733305258]
Robot Arm 1 togoal: 26.8423575600939
Mobile Platform 2 Pose: [10.6586901223807;-0.0543889717808433;4.1152618553491]
Robot Arm 2 end: [-0.0109487120286101;3.3505870552269;0.879922373582271]
Robot Arm 1 togoal: 16.686230876864

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.776854e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-1.73869917274382;-8.48601988480017;-9.65736069039538]
Robot Arm 1 end: [-8.35080309107887;-9.41332313079736;0.214739370454711]
Robot Arm 1 togoal: 26.7457549136419
Mobile Platform 2 Pose: [10.6120048262339;-0.0712189552026869;4.05225707066847]
Robot Arm 2 end: [0.359375286081055;3.53608622955273;1.02877561128742]
Robot Arm 1 togoal: 17.0524066213716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.750922e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-1.78084204190661;-8.45989521226616;-9.65805698895692]
Robot Arm 1 end: [-8.24601016610736;-9.4413535633823;0.302493246357371]
Robot Arm 1 togoal: 26.6902205698417
Mobile Platform 2 Pose: [10.51264725586;-0.0334776507741906;4.02814504709999]
Robot Arm 2 end: [0.328739768248171;3.36219608675181;0.884553644510631]
Robot Arm 1 togoal: 16.9007755273068

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.849778e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-1.80097713881494;-8.44301647827201;-9.65718545377921]
Robot Arm 1 end: [-8.23192595772298;-9.48624565566074;0.254758213196351]
Robot Arm 1 togoal: 26.7155434558853
Mobile Platform 2 Pose: [10.2971179925922;0.0512369156371713;3.96712724521133]
Robot Arm 2 end: [0.198532155247584;3.22587572840143;0.944669864173795]
Robot Arm 1 togoal: 16.7112537928846

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.276914e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-1.81435840745962;-8.42973071474857;-9.65598482507919]
Robot Arm 1 end: [-8.25242928884713;-9.53411193453472;0.208434992944789]
Robot Arm 1 togoal: 26.7666869618382
Mobile Platform 2 Pose: [10.233631758169;0.063569488555165;3.90505798784694]
Robot Arm 2 end: [0.351657346079498;3.31228790596029;1.08963345313913]
Robot Arm 1 togoal: 16.868903230488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.703075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-1.84184115826916;-8.41080951839536;-9.65535083347304]
Robot Arm 1 end: [-8.1805926153801;-9.52295759963406;0.25536806733743]
Robot Arm 1 togoal: 26.7073331778465
Mobile Platform 2 Pose: [10.124246760742;0.100281636161741;3.8622437471492]
Robot Arm 2 end: [0.659824274509767;3.33364302096842;0.819206778957688]
Robot Arm 1 togoal: 17.0853856701904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.661100e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-1.88167781444203;-8.38392957736702;-9.65494799175988]
Robot Arm 1 end: [-8.10791892415399;-9.51996127785839;0.278561632053293]
Robot Arm 1 togoal: 26.6546294131239
Mobile Platform 2 Pose: [10.0304756105945;0.145529531604105;3.8442040940802]
Robot Arm 2 end: [0.560561342953282;3.16824204782383;0.809266601102601]
Robot Arm 1 togoal: 16.8948104144279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-1.92390431459981;-8.35520031368936;-9.65461509434122]
Robot Arm 1 end: [-8.03895913459367;-9.50964699111918;0.323387223593937]
Robot Arm 1 togoal: 26.5982076226921
Mobile Platform 2 Pose: [9.86303774591651;0.235140553407576;3.78888392495423]
Robot Arm 2 end: [0.746481925247507;3.26077248927104;0.95867411354151]
Robot Arm 1 togoal: 17.0777976093073

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975928e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-1.96958612049856;-8.32413404541245;-9.65420070186145]
Robot Arm 1 end: [-7.95957501111492;-9.51013166133014;0.309554591795614]
Robot Arm 1 togoal: 26.5454171562505
Mobile Platform 2 Pose: [9.63670442994477;0.356677491455948;3.74026669324597]
Robot Arm 2 end: [0.983426633910669;3.26714164121758;0.808505968441454]
Robot Arm 1 togoal: 17.2383397024085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.857201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-2.00779644493301;-8.29754020319253;-9.65370284591717]
Robot Arm 1 end: [-7.90452085769436;-9.51536461087009;0.273094322723144]
Robot Arm 1 togoal: 26.5137434478459
Mobile Platform 2 Pose: [9.45962040252186;0.451853915400699;3.71957362674414]
Robot Arm 2 end: [1.20834106068714;3.41228175187504;0.677271459732384]
Robot Arm 1 togoal: 17.4993739594886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981787e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-2.05529806137465;-8.26676617778964;-9.6531361827294]
Robot Arm 1 end: [-7.91585619280356;-9.44493504560182;0.209089830531103]
Robot Arm 1 togoal: 26.4727125610816
Mobile Platform 2 Pose: [9.43446756009667;0.465609742799143;3.71853622329374]
Robot Arm 2 end: [1.19998918864973;3.36990157512177;0.68063845391412]
Robot Arm 1 togoal: 17.4613926558695

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-2.099131928846;-8.23852517297142;-9.65162472378539]
Robot Arm 1 end: [-7.94656719584964;-9.39753682165403;0.15216870930752]
Robot Arm 1 togoal: 26.4616214160234
Mobile Platform 2 Pose: [9.28779996876988;0.542013928070965;3.71561009391411]
Robot Arm 2 end: [1.24513309300979;3.32283306373167;0.664278513940719]
Robot Arm 1 togoal: 17.4552247429886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-2.13921158128954;-8.2127257971088;-9.64915287145049]
Robot Arm 1 end: [-7.98837735090507;-9.37012761661064;0.100745665825061]
Robot Arm 1 togoal: 26.4726403287636
Mobile Platform 2 Pose: [9.20349669603518;0.592825054550267;3.70762019968129]
Robot Arm 2 end: [1.36222575984271;3.49542611118772;0.703908922465261]
Robot Arm 1 togoal: 17.6605305121094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-2.17612350685468;-8.18907032494893;-9.64592305422469]
Robot Arm 1 end: [-8.04392416454167;-9.37321611196096;0.0455169789546454]
Robot Arm 1 togoal: 26.5156887883733
Mobile Platform 2 Pose: [9.15556439099369;0.617677346203187;3.70696457964527]
Robot Arm 2 end: [1.36870515894735;3.44774404104532;0.689395816766013]
Robot Arm 1 togoal: 17.6289773470306

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968835e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-2.20757957823441;-8.16900067029179;-9.64242149288363]
Robot Arm 1 end: [-8.10528274527731;-9.38861983024195;0.00608734737604971]
Robot Arm 1 togoal: 26.5709390604316
Mobile Platform 2 Pose: [9.09221140253808;0.652819027047125;3.70397462564475]
Robot Arm 2 end: [1.37446176119314;3.38192763036759;0.682112170679462]
Robot Arm 1 togoal: 17.5828922607011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-2.2333693501785;-8.15271571415169;-9.63906137558259]
Robot Arm 1 end: [-8.174610007542;-9.41304960361149;-0.0132340945805882]
Robot Arm 1 togoal: 26.6371323174155
Mobile Platform 2 Pose: [9.01479725411449;0.699628640412627;3.69626606957865]
Robot Arm 2 end: [1.47040321308399;3.53075788066916;0.63443189683551]
Robot Arm 1 togoal: 17.7605619758376

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-2.25453498718015;-8.13957359758553;-9.63615068303023]
Robot Arm 1 end: [-8.25382026083222;-9.43995590534533;-0.00818001371392679]
Robot Arm 1 togoal: 26.7105265996551
Mobile Platform 2 Pose: [8.95580927943804;0.729559624557435;3.695944980275]
Robot Arm 2 end: [1.48431924409983;3.49054141997792;0.612568228287503]
Robot Arm 1 togoal: 17.7400548079311

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-2.27166139321466;-8.12918033565512;-9.63382363529825]
Robot Arm 1 end: [-8.14075559065765;-9.39740667478869;-0.00612981123425843]
Robot Arm 1 togoal: 26.6022278121114
Mobile Platform 2 Pose: [8.87253745739258;0.77371185798754;3.69367669659292]
Robot Arm 2 end: [1.50647572446588;3.44023078366831;0.593423115772774]
Robot Arm 1 togoal: 17.7172002865052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-2.29192300919329;-8.11647573404212;-9.6306622783483]
Robot Arm 1 end: [-8.1961943167625;-9.42796796236229;-0.0306344363356318]
Robot Arm 1 togoal: 26.6637393511109
Mobile Platform 2 Pose: [8.81382591383163;0.808498431213411;3.68827454959664]
Robot Arm 2 end: [1.54813461765547;3.5006389687844;0.530573507567802]
Robot Arm 1 togoal: 17.7934710034289

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-2.30937566933689;-8.10566591060828;-9.62794052899228]
Robot Arm 1 end: [-8.24767649775163;-9.47823111112036;-0.0510132030100813]
Robot Arm 1 togoal: 26.7366865990394
Mobile Platform 2 Pose: [8.71753828849661;0.859397720058987;3.68554621266083]
Robot Arm 2 end: [1.58012286927574;3.47176199360424;0.507567950138014]
Robot Arm 1 togoal: 17.7936691381944

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999850e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-2.32646264384672;-8.09523012081462;-9.62574190859858]
Robot Arm 1 end: [-8.13665000337572;-9.41659460954;-0.0520539566424091]
Robot Arm 1 togoal: 26.6160988562688
Mobile Platform 2 Pose: [8.66506366080018;0.889922512876068;3.68081417418329]
Robot Arm 2 end: [1.58205154331479;3.49037497175594;0.392625636917423]
Robot Arm 1 togoal: 17.8159300496449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-2.34366992139683;-8.08446334429107;-9.62275526744031]
Robot Arm 1 end: [-8.17820777226823;-9.46730312737579;-0.0744975762726421]
Robot Arm 1 togoal: 26.6827423886822
Mobile Platform 2 Pose: [8.64098711220697;0.904126353615398;3.67877780402579]
Robot Arm 2 end: [1.56872269331821;3.47458422781184;0.344659183545852]
Robot Arm 1 togoal: 17.798414046868

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000308e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.611399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-2.36018405828197;-8.07430674667331;-9.62018642155277]
Robot Arm 1 end: [-8.13097872347868;-9.45111176483114;-0.0805564387686736]
Robot Arm 1 togoal: 26.6391276393532
Mobile Platform 2 Pose: [8.62846981768287;0.911980879354357;3.67749446064258]
Robot Arm 2 end: [1.49139040982868;3.42032294170567;0.249622635508734]
Robot Arm 1 togoal: 17.7136535422175

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983144e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.635684e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-2.41008748861168;-8.04739826732862;-9.61744421110811]
Robot Arm 1 end: [-8.16133968532261;-9.54904600460471;-0.104329127862847]
Robot Arm 1 togoal: 26.7327730254402
Mobile Platform 2 Pose: [8.6220592815144;0.916494815937694;3.67680358124339]
Robot Arm 2 end: [1.5104814374491;3.45244514814049;0.258875082151707]
Robot Arm 1 togoal: 17.749716129951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979717e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-2.46264883569034;-8.019098572937;-9.61537477998334]
Robot Arm 1 end: [-8.21669662124413;-9.68086381322881;-0.096639884705484]
Robot Arm 1 togoal: 26.866358052812
Mobile Platform 2 Pose: [8.61719439798625;0.9198453783444;3.67632638142349]
Robot Arm 2 end: [1.52186174593397;3.48028897337503;0.252473392983008]
Robot Arm 1 togoal: 17.7786571104463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.964512e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-2.55914297138173;-7.96808383754697;-9.61414505066852]
Robot Arm 1 end: [-8.23108108586983;-9.74064951229321;-0.0661448232404376]
Robot Arm 1 togoal: 26.9181089494916
Mobile Platform 2 Pose: [8.6184162754437;0.919304394660672;3.67654742591201]
Robot Arm 2 end: [1.50201463172889;3.47991908751613;0.231546669538681]
Robot Arm 1 togoal: 17.7670270187762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-2.67914002279598;-7.90463597495639;-9.61293727508446]
Robot Arm 1 end: [-8.24702679279801;-9.85054399283352;-0.00295684600183471]
Robot Arm 1 togoal: 27.005935052447
Mobile Platform 2 Pose: [8.62123011860567;0.917715003526725;3.67693782613146]
Robot Arm 2 end: [1.49416190282507;3.47978542438672;0.231117623784232]
Robot Arm 1 togoal: 17.7618740419004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-2.74412000319683;-7.86980288867204;-9.6121533988204]
Robot Arm 1 end: [-8.28068515999127;-9.9832195980275;0.11264642126727]
Robot Arm 1 togoal: 27.1200290417331
Mobile Platform 2 Pose: [8.62436796792137;0.915780962142923;3.6773126887874]
Robot Arm 2 end: [1.49995431614437;3.48917433983042;0.245582014507606]
Robot Arm 1 togoal: 17.7717040455175

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973720e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-2.84230956139049;-7.81968656295299;-9.612395608588]
Robot Arm 1 end: [-8.3525493348921;-9.96732268489371;0.220825817595023]
Robot Arm 1 togoal: 27.1514363376399
Mobile Platform 2 Pose: [8.62714437271446;0.913757683828815;3.67750582866389]
Robot Arm 2 end: [1.51378608677912;3.51292489183957;0.265163410923584]
Robot Arm 1 togoal: 17.7972900949293

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-2.89160998864131;-7.79432590042663;-9.61250052227564]
Robot Arm 1 end: [-8.33591505202427;-10.0305006333857;0.302874538906507]
Robot Arm 1 togoal: 27.1829094479984
Mobile Platform 2 Pose: [8.6267624767262;0.913063031340489;3.67706020980229]
Robot Arm 2 end: [1.53472674126564;3.55390982285602;0.286997695290013]
Robot Arm 1 togoal: 17.8404359299265

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-2.93857757956911;-7.77034979922694;-9.61263078636395]
Robot Arm 1 end: [-8.29050515959658;-10.0681469712213;0.325801219983426]
Robot Arm 1 togoal: 27.1790655096072
Mobile Platform 2 Pose: [8.62389093952111;0.913684958176217;3.67623764424596]
Robot Arm 2 end: [1.55592393759638;3.5901648115838;0.305295677759338]
Robot Arm 1 togoal: 17.8804332905173

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-2.98362558770028;-7.74738993627143;-9.6128452834756]
Robot Arm 1 end: [-8.28287948902659;-10.0471791468805;0.321593490623073]
Robot Arm 1 togoal: 27.1586385912867
Mobile Platform 2 Pose: [8.62526619506286;0.912309797406294;3.67608448851296]
Robot Arm 2 end: [1.57138873144712;3.6069462202466;0.316930796221513]
Robot Arm 1 togoal: 17.9023966345971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-3.02839235869906;-7.72457117025086;-9.61306226727302]
Robot Arm 1 end: [-8.27614860015323;-10.0266819165171;0.321776788674398]
Robot Arm 1 togoal: 27.1389707349195
Mobile Platform 2 Pose: [8.63193885893267;0.908085602643822;3.67656020858204]
Robot Arm 2 end: [1.58110447491031;3.6145592155885;0.324584583987152]
Robot Arm 1 togoal: 17.9139502439084

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-3.07252737122768;-7.70207425328822;-9.61327501650214]
Robot Arm 1 end: [-8.23861023758265;-10.0503912314784;0.342333275928108]
Robot Arm 1 togoal: 27.130321545832
Mobile Platform 2 Pose: [8.64164563675933;0.90207473773723;3.67729599440006]
Robot Arm 2 end: [1.58681810216426;3.62076932458127;0.330138186639165]
Robot Arm 1 togoal: 17.9219938228038

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-3.16279437578783;-7.6565192625392;-9.61361217094995]
Robot Arm 1 end: [-8.22693483886954;-10.0212652447418;0.354312332269937]
Robot Arm 1 togoal: 27.1004312831869
Mobile Platform 2 Pose: [8.65383807708477;0.894438259487817;3.67815127962936]
Robot Arm 2 end: [1.58825939698696;3.62827399331617;0.333987105209581]
Robot Arm 1 togoal: 17.9283743686134

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988583e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-3.24123299750194;-7.61696612949082;-9.61389986976081]
Robot Arm 1 end: [-8.21747272524767;-9.98815216100971;0.371723882065808]
Robot Arm 1 togoal: 27.0688635687218
Mobile Platform 2 Pose: [8.66821019578364;0.885187374909635;3.67900392820645]
Robot Arm 2 end: [1.58488360033961;3.63938472740825;0.336338635674073]
Robot Arm 1 togoal: 17.9344853933365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-3.30609038051917;-7.58428516453611;-9.61410109857232]
Robot Arm 1 end: [-8.20895833574459;-9.95563451761548;0.394074367827154]
Robot Arm 1 togoal: 27.0381854800572
Mobile Platform 2 Pose: [8.68442979265978;0.874307544875405;3.67969712608075]
Robot Arm 2 end: [1.57594799745255;3.65695669216178;0.337141384282814]
Robot Arm 1 togoal: 17.9420333115652

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994817e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-3.35675342216641;-7.55875766221204;-9.6142117123678]
Robot Arm 1 end: [-8.19948379830333;-9.93094472811009;0.423196077909573]
Robot Arm 1 togoal: 27.0123849693255
Mobile Platform 2 Pose: [8.70193533175348;0.861861057162219;3.67999125098635]
Robot Arm 2 end: [1.56034655315182;3.68558789891291;0.335935584578768]
Robot Arm 1 togoal: 17.9538660526511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997981e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-3.36668490484613;-7.55359535922369;-9.61424107094449]
Robot Arm 1 end: [-8.18046283692024;-9.95332573619554;0.440401757504893]
Robot Arm 1 togoal: 27.0154054098961
Mobile Platform 2 Pose: [8.71965498072748;0.848155732555812;3.67949546764606]
Robot Arm 2 end: [1.53568779227618;3.73348186648975;0.331679983575596]
Robot Arm 1 togoal: 17.974846289177

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-3.37325656776966;-7.5501752122393;-9.61430944261878]
Robot Arm 1 end: [-8.16380444706468;-9.97450170560237;0.452600335792645]
Robot Arm 1 togoal: 27.0193632090173
Mobile Platform 2 Pose: [8.7352634545577;0.834232962282061;3.67754238663503]
Robot Arm 2 end: [1.4948335412909;3.81449932215862;0.322564643376432]
Robot Arm 1 togoal: 18.0113252418558

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998990e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-3.37617361158358;-7.54878255774263;-9.61441894945629]
Robot Arm 1 end: [-8.15898781516064;-9.9768145404884;0.457464445765576]
Robot Arm 1 togoal: 27.0176435969413
Mobile Platform 2 Pose: [8.74274658494837;0.823519329451435;3.6730051933266]
Robot Arm 2 end: [1.43296016121471;3.91594374007326;0.305639981728232]
Robot Arm 1 togoal: 18.0511265077507

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000106e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-3.37621732515855;-7.54910212368604;-9.61443276331995]
Robot Arm 1 end: [-8.16746685318209;-9.9486602000778;0.455197124131734]
Robot Arm 1 togoal: 27.0026263798514
Mobile Platform 2 Pose: [8.7359288666945;0.820683847465131;3.66572098348531]
Robot Arm 2 end: [1.36840746582532;3.99363250226944;0.27929456697491]
Robot Arm 1 togoal: 18.0721960840405

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000646e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-3.37356155892492;-7.55099956583343;-9.61431540524228]
Robot Arm 1 end: [-8.18349648178837;-9.91259932034336;0.45035026907731]
Robot Arm 1 togoal: 26.9869866202597
Mobile Platform 2 Pose: [8.72809761249189;0.819226085091634;3.65779426947477]
Robot Arm 2 end: [1.28961283636586;4.04398514548277;0.244802431853337]
Robot Arm 1 togoal: 18.0642618723966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001480e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-3.36940815449869;-7.55386771518557;-9.61378732579351]
Robot Arm 1 end: [-8.20621822863034;-9.84852092048642;0.439269822853009]
Robot Arm 1 togoal: 26.9555245746702
Mobile Platform 2 Pose: [8.72881137320463;0.813429328295832;3.64949711958084]
Robot Arm 2 end: [1.19213648791796;4.05611338160544;0.199418508277357]
Robot Arm 1 togoal: 18.0162376816026

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001356e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-3.36349124341923;-7.55753206831782;-9.61298122731995]
Robot Arm 1 end: [-8.21662878878008;-9.77683353189885;0.426262464542754]
Robot Arm 1 togoal: 26.910354127142
Mobile Platform 2 Pose: [8.7430974966585;0.800110982186213;3.64048651939267]
Robot Arm 2 end: [1.0741553514094;4.03139384333772;0.152834304309137]
Robot Arm 1 togoal: 17.9273201372896

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001253e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-3.35317708334687;-7.56332444572414;-9.61224213671209]
Robot Arm 1 end: [-8.21657491867973;-9.72938060254241;0.421896228853283]
Robot Arm 1 togoal: 26.8756411984458
Mobile Platform 2 Pose: [8.76066424160276;0.785417910897079;3.6301589985957]
Robot Arm 2 end: [0.944304641555041;3.97975852274196;0.116181407280396]
Robot Arm 1 togoal: 17.8096874683356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-3.34731719200406;-7.56694771164439;-9.61144787054925]
Robot Arm 1 end: [-8.21659320635618;-9.65127700992592;0.434816425643893]
Robot Arm 1 togoal: 26.8178438902889
Mobile Platform 2 Pose: [8.76707734969863;0.778553963478549;3.61861857872005]
Robot Arm 2 end: [0.820574300605416;3.89787181216098;0.0982373925570562]
Robot Arm 1 togoal: 17.6708778089635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-3.3376228920423;-7.57217719913441;-9.61103511918755]
Robot Arm 1 end: [-8.21165126331476;-9.6378260115465;0.454037038082538]
Robot Arm 1 togoal: 26.8038590907815
Mobile Platform 2 Pose: [8.75621213993934;0.784063771954359;3.60769664018906]
Robot Arm 2 end: [0.715974950106251;3.78866706098354;0.107447028466954]
Robot Arm 1 togoal: 17.5202052057985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-3.32631974460029;-7.57813218744024;-9.61067828582374]
Robot Arm 1 end: [-8.20371053981612;-9.64350064619088;0.470675244477934]
Robot Arm 1 togoal: 26.8019659194317
Mobile Platform 2 Pose: [8.75627957964942;0.784472588840224;3.59987000593391]
Robot Arm 2 end: [0.669029046930859;3.69845759935177;0.141617489406339]
Robot Arm 1 togoal: 17.4177672942322

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000559e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-3.2988450741958;-7.59231132046308;-9.61030109849738]
Robot Arm 1 end: [-8.18426628906301;-9.71081162147728;0.482371550948922]
Robot Arm 1 togoal: 26.8377045274758
Mobile Platform 2 Pose: [8.77958422695872;0.770381436649568;3.59384004884081]
Robot Arm 2 end: [0.600725439269274;3.65268774005118;0.179997237267177]
Robot Arm 1 togoal: 17.3369417942084

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000518e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-3.2736406247249;-7.60721466229662;-9.6082978328594]
Robot Arm 1 end: [-8.16416538752916;-9.68309621685652;0.489127548076127]
Robot Arm 1 togoal: 26.8034688364134
Mobile Platform 2 Pose: [8.81570545973559;0.747878370333024;3.58970431387084]
Robot Arm 2 end: [0.523556163221374;3.63403798277318;0.225348663766656]
Robot Arm 1 togoal: 17.2716802431396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001597e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-3.28008981524317;-7.60709658356504;-9.60502416398785]
Robot Arm 1 end: [-8.12132296774646;-9.65555252785197;0.486844521522904]
Robot Arm 1 togoal: 26.7543000063576
Mobile Platform 2 Pose: [8.84455942076176;0.729430520960666;3.58843831397888]
Robot Arm 2 end: [0.455522410186563;3.63464259435324;0.255243751577195]
Robot Arm 1 togoal: 17.2285683220077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001188e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-3.28709740708569;-7.60598185727005;-9.60181618181385]
Robot Arm 1 end: [-8.05909859965386;-9.6793764303551;0.462536322564295]
Robot Arm 1 togoal: 26.7306798319101
Mobile Platform 2 Pose: [8.86129232278293;0.718732892393988;3.58792316351713]
Robot Arm 2 end: [0.421893871739329;3.65580674884122;0.271780363051493]
Robot Arm 1 togoal: 17.2237475462465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000874e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-3.29754308957169;-7.60183399343887;-9.59875782981983]
Robot Arm 1 end: [-7.96956467962347;-9.73938779902857;0.432623095660376]
Robot Arm 1 togoal: 26.7157865267244
Mobile Platform 2 Pose: [8.87744116799011;0.708947801046812;3.5864909016106]
Robot Arm 2 end: [0.438715393002769;3.67390637595533;0.2674893594802]
Robot Arm 1 togoal: 17.2485878714868

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000917e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-3.31119852897802;-7.59538320219335;-9.59496912892357]
Robot Arm 1 end: [-7.83825838477144;-9.80071367200016;0.406068198174257]
Robot Arm 1 togoal: 26.6742347040196
Mobile Platform 2 Pose: [8.90260613515568;0.693671786392516;3.58423012488815]
Robot Arm 2 end: [0.491671729752049;3.68259651934809;0.246177362848575]
Robot Arm 1 togoal: 17.2891236317041

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000902e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-3.32251422391806;-7.59030815769501;-9.58982870214616]
Robot Arm 1 end: [-7.69598631607639;-9.82448631186459;0.415407136868819]
Robot Arm 1 togoal: 26.5966261921898
Mobile Platform 2 Pose: [8.93912105613887;0.670844860585029;3.58198052765347]
Robot Arm 2 end: [0.558644402562695;3.68270021309489;0.226341220258567]
Robot Arm 1 togoal: 17.3313991961745

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001784e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-3.32340242425992;-7.59152559868553;-9.58422240056199]
Robot Arm 1 end: [-7.61370829921171;-9.81406223716176;0.411318489135432]
Robot Arm 1 togoal: 26.5343395516714
Mobile Platform 2 Pose: [8.98392099254472;0.642090385619081;3.58080700341512]
Robot Arm 2 end: [0.617325755769952;3.67237014184542;0.199146269760799]
Robot Arm 1 togoal: 17.3611234885277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000826e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-3.31818230857731;-7.59613905638028;-9.57595460801823]
Robot Arm 1 end: [-7.56899581475968;-9.77097131347154;0.423724682391699]
Robot Arm 1 togoal: 26.4719650524813
Mobile Platform 2 Pose: [9.02172238454523;0.617467853244014;3.58064978600053]
Robot Arm 2 end: [0.688323590035323;3.64978748067908;0.173590612110088]
Robot Arm 1 togoal: 17.3888605575979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001219e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-3.32029492732148;-7.5934934673123;-9.56830593342234]
Robot Arm 1 end: [-7.58441885769943;-9.70809350892936;0.351494111547802]
Robot Arm 1 togoal: 26.4383902751821
Mobile Platform 2 Pose: [9.04081617785146;0.604824692722039;3.58056677805562]
Robot Arm 2 end: [0.778972446551104;3.60559993371968;0.161531069831009]
Robot Arm 1 togoal: 17.4111100258437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000313e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-3.35422323447344;-7.56745025331107;-9.55573267705975]
Robot Arm 1 end: [-7.56428884848229;-9.61741662168802;0.325704620186834]
Robot Arm 1 togoal: 26.3586040735621
Mobile Platform 2 Pose: [9.05009397032559;0.598102579209328;3.57942362434787]
Robot Arm 2 end: [0.846371718714799;3.56595616798012;0.156716465965273]
Robot Arm 1 togoal: 17.422405567927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997158e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-3.40351716575254;-7.53258032033215;-9.55043723216972]
Robot Arm 1 end: [-7.51570716627482;-9.52785228246185;0.313580905933076]
Robot Arm 1 togoal: 26.2601384834326
Mobile Platform 2 Pose: [9.05478656718873;0.593312745822598;3.57666089289049]
Robot Arm 2 end: [0.901447540060727;3.56663175546443;0.150481299741]
Robot Arm 1 togoal: 17.4577597980048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-3.43681935989744;-7.51010287143474;-9.54776147070521]
Robot Arm 1 end: [-7.50704104885514;-9.40350705925306;0.293640944512181]
Robot Arm 1 togoal: 26.1629314629663
Mobile Platform 2 Pose: [9.05596113858212;0.591345449903524;3.57499694757915]
Robot Arm 2 end: [0.937361163628676;3.57567589174503;0.148987802158139]
Robot Arm 1 togoal: 17.4873457852353

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984314e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-3.46208508185379;-7.4939466547741;-9.54561947100018]
Robot Arm 1 end: [-7.56572961559532;-9.28754915975526;0.306882315103876]
Robot Arm 1 togoal: 26.1158666574739
Mobile Platform 2 Pose: [9.0637134569894;0.586091899335725;3.57524008237368]
Robot Arm 2 end: [0.929202151031228;3.56811152404041;0.155185327327047]
Robot Arm 1 togoal: 17.4758850908042

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973794e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.442107e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-3.50812963372592;-7.46879980095682;-9.54441507859369]
Robot Arm 1 end: [-7.64319192632935;-9.26796005351075;0.28586366160761]
Robot Arm 1 togoal: 26.1545802441446
Mobile Platform 2 Pose: [9.07488471915445;0.578416855326602;3.57550205236287]
Robot Arm 2 end: [0.924827056486939;3.56238136087963;0.178748510467145]
Robot Arm 1 togoal: 17.4668736027154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967617e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.330330e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-3.56064446809877;-7.44029115957637;-9.541281948554]
Robot Arm 1 end: [-7.67677824582408;-9.34790144856114;0.258043965465386]
Robot Arm 1 togoal: 26.2374701059466
Mobile Platform 2 Pose: [9.0836003864086;0.571999089194701;3.57520778838628]
Robot Arm 2 end: [0.901005099774593;3.51822861060492;0.213331749286076]
Robot Arm 1 togoal: 17.4150628252618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.226463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-3.60236875524845;-7.41895844268545;-9.53713184368289]
Robot Arm 1 end: [-7.6902486698131;-9.4533418053213;0.273489859547012]
Robot Arm 1 togoal: 26.3236336197155
Mobile Platform 2 Pose: [9.08655604079994;0.569024796928727;3.57422400320567]
Robot Arm 2 end: [0.886246095696341;3.47088367728727;0.252579217994463]
Robot Arm 1 togoal: 17.3661726463632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983082e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003402e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-3.64035697503889;-7.39963913910842;-9.53386963144106]
Robot Arm 1 end: [-7.71780006954752;-9.52971133914775;0.323856975073071]
Robot Arm 1 togoal: 26.3962727618318
Mobile Platform 2 Pose: [9.08105446473156;0.570612748419684;3.57281281773596]
Robot Arm 2 end: [0.888065955134789;3.4310685903732;0.296767015705834]
Robot Arm 1 togoal: 17.3332723431702

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.693829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-3.67919946695186;-7.37960722989168;-9.53049296845855]
Robot Arm 1 end: [-7.75807340108134;-9.56619272319916;0.377187354129457]
Robot Arm 1 togoal: 26.4479370834176
Mobile Platform 2 Pose: [9.08440482836698;0.568013737531413;3.57118649834664]
Robot Arm 2 end: [0.938602113463737;3.44665150161771;0.331415653816066]
Robot Arm 1 togoal: 17.3747104643305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999038e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-3.72112106576197;-7.35809137177663;-9.52652968138893]
Robot Arm 1 end: [-7.80225065994255;-9.58486479075352;0.419190880101993]
Robot Arm 1 togoal: 26.4896402536579
Mobile Platform 2 Pose: [9.08914140220749;0.564315954243536;3.57119466237539]
Robot Arm 2 end: [0.969504256031709;3.48866349047116;0.345462220673823]
Robot Arm 1 togoal: 17.4257268271051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-3.77076162918409;-7.33273595277407;-9.52151878095851]
Robot Arm 1 end: [-7.83387368384335;-9.59592076322179;0.441182144591364]
Robot Arm 1 togoal: 26.5181667490535
Mobile Platform 2 Pose: [9.1051563092861;0.552614259706889;3.5715350302083]
Robot Arm 2 end: [0.943800165245613;3.52584649763716;0.339648944092162]
Robot Arm 1 togoal: 17.4387750674114

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997316e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-3.82532331190814;-7.30501082913844;-9.51585857123544]
Robot Arm 1 end: [-7.84796610728396;-9.58556973565826;0.431306204088879]
Robot Arm 1 togoal: 26.5204047594666
Mobile Platform 2 Pose: [9.12617725689055;0.536142606438539;3.57034577887076]
Robot Arm 2 end: [0.879460489315948;3.55391483259758;0.320839307631941]
Robot Arm 1 togoal: 17.421585849612

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997565e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-3.87959712077852;-7.27759320616135;-9.51026834674914]
Robot Arm 1 end: [-7.84947501211634;-9.56255969573127;0.398996307308813]
Robot Arm 1 togoal: 26.5057803571905
Mobile Platform 2 Pose: [9.14810874903124;0.518055299686342;3.56712422641207]
Robot Arm 2 end: [0.781779737586987;3.57568860498712;0.296273025315979]
Robot Arm 1 togoal: 17.3794721315677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-3.93144564920661;-7.25142091121725;-9.50486380914487]
Robot Arm 1 end: [-7.83983536735907;-9.5318014812114;0.351654597911416]
Robot Arm 1 togoal: 26.4786397631244
Mobile Platform 2 Pose: [9.17074408996398;0.49887873176129;3.5608995133484]
Robot Arm 2 end: [0.679233303639;3.58823080898372;0.280024179258794]
Robot Arm 1 togoal: 17.3270326790086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-3.97836331587908;-7.22766114352737;-9.49969523130642]
Robot Arm 1 end: [-7.81117322910083;-9.48867322936666;0.290780709616114]
Robot Arm 1 togoal: 26.4302802773732
Mobile Platform 2 Pose: [9.19158330227639;0.480748756961189;3.55012095099355]
Robot Arm 2 end: [0.576128358180123;3.59233865447444;0.255074649747078]
Robot Arm 1 togoal: 17.2687210915874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-4.0197071197039;-7.20659938738952;-9.49435635778762]
Robot Arm 1 end: [-7.75762679076253;-9.40995450050216;0.222114009441097]
Robot Arm 1 togoal: 26.3394523057721
Mobile Platform 2 Pose: [9.20189563022701;0.470949488913183;3.53509391971835]
Robot Arm 2 end: [0.470978532516539;3.57952641117502;0.233586072834818]
Robot Arm 1 togoal: 17.1959984763921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999448e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-4.06194317891689;-7.18492863738211;-9.48752271167345]
Robot Arm 1 end: [-7.64076949146314;-9.29499247593297;0.133432816581909]
Robot Arm 1 togoal: 26.1805046854667
Mobile Platform 2 Pose: [9.20654774837349;0.467680060036905;3.52186624958882]
Robot Arm 2 end: [0.375543118723606;3.55848463175628;0.23668664307057]
Robot Arm 1 togoal: 17.1211524529411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995041e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-4.11574142762169;-7.15748533145437;-9.47542213320954]
Robot Arm 1 end: [-7.54981987005397;-9.15605853937557;0.11036425256099]
Robot Arm 1 togoal: 26.0180831239982
Mobile Platform 2 Pose: [9.23777866235653;0.448169754692;3.51404927149735]
Robot Arm 2 end: [0.273532809719409;3.55496086810524;0.277089946973578]
Robot Arm 1 togoal: 17.0537313438516

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000763e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-4.16239710973953;-7.13487665463671;-9.46024968015686]
Robot Arm 1 end: [-7.48926984522801;-9.00059628203893;0.16779214434146]
Robot Arm 1 togoal: 25.8597832607477
Mobile Platform 2 Pose: [9.26686736825234;0.429816358152737;3.51131796289665]
Robot Arm 2 end: [0.199550977977116;3.57078883316026;0.320162669649846]
Robot Arm 1 togoal: 17.0187884410302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002531e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-4.2116401302535;-7.11409125733714;-9.43482371155323]
Robot Arm 1 end: [-7.42931803735406;-8.81544380258737;0.334820238000833]
Robot Arm 1 togoal: 25.6750671890715
Mobile Platform 2 Pose: [9.28815050069095;0.416626472699556;3.50949303323404]
Robot Arm 2 end: [0.195033318287563;3.57451304425912;0.331635870973173]
Robot Arm 1 togoal: 17.0182459666433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007185e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-4.16490057582312;-7.18114411947478;-9.30807436853306]
Robot Arm 1 end: [-7.39030534102504;-8.62593058431076;0.364974844806617]
Robot Arm 1 togoal: 25.5085163062278
Mobile Platform 2 Pose: [9.31282617849169;0.401364278065609;3.50725283690366]
Robot Arm 2 end: [0.230362267263679;3.55633909391096;0.325050190077066]
Robot Arm 1 togoal: 17.0254203383178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.913431e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-4.0045259391366;-7.27997785894215;-9.27229434650643]
Robot Arm 1 end: [-7.63499795669141;-8.6066874751244;0.512397675840426]
Robot Arm 1 togoal: 25.6557839556261
Mobile Platform 2 Pose: [9.32765098165391;0.390213677109071;3.5046470516053]
Robot Arm 2 end: [0.273841383480462;3.53793352358055;0.292111550426992]
Robot Arm 1 togoal: 17.0392965724321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005333e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000785e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-4.06885859901637;-7.25768885248299;-9.22520116512193]
Robot Arm 1 end: [-7.45032185875592;-8.54179095353691;0.309467270721589]
Robot Arm 1 togoal: 25.4907739445657
Mobile Platform 2 Pose: [9.35105024179711;0.373636069333482;3.50175930441584]
Robot Arm 2 end: [0.32181069534679;3.52104953360125;0.272424923628224]
Robot Arm 1 togoal: 17.0563037098315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.825180e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005514e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-4.02249262195619;-7.29117992804311;-9.19616421190833]
Robot Arm 1 end: [-7.37206544597077;-8.45261852483978;0.486312452690436]
Robot Arm 1 togoal: 25.3645099815256
Mobile Platform 2 Pose: [9.37721517707809;0.354631596582751;3.49914786360102]
Robot Arm 2 end: [0.367880175390259;3.49110097954235;0.269337666360766]
Robot Arm 1 togoal: 17.0607562425852

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.717040e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011282e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-3.94487030494605;-7.33905655519912;-9.16889924665542]
Robot Arm 1 end: [-7.53623486710023;-8.49820279765473;0.336577092775637]
Robot Arm 1 togoal: 25.5167767886232
Mobile Platform 2 Pose: [9.39979067103506;0.337603455612845;3.49717876137314]
Robot Arm 2 end: [0.419574276119973;3.44989677875319;0.265018672792957]
Robot Arm 1 togoal: 17.0600334275461

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.827819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012799e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-4.06315161102005;-7.30395646245521;-9.01227437298994]
Robot Arm 1 end: [-7.39752096893218;-8.28041197842832;0.532777357241622]
Robot Arm 1 togoal: 25.2551733701664
Mobile Platform 2 Pose: [9.42108196597614;0.320885712070663;3.49568353662925]
Robot Arm 2 end: [0.50273457245551;3.43831945708457;0.26149515997563]
Robot Arm 1 togoal: 17.1021114419289

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.911214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003263e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-4.04718252021606;-7.31512745603976;-8.99689624929224]
Robot Arm 1 end: [-7.51416667224064;-8.31743428137812;0.355306634917113]
Robot Arm 1 togoal: 25.3699285355128
Mobile Platform 2 Pose: [9.43559970287787;0.30848844881774;3.49400451910067]
Robot Arm 2 end: [0.607719036615487;3.48418177758667;0.285476409550231]
Robot Arm 1 togoal: 17.2009703189481

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.765228e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943764e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-4.22891153391967;-7.24176131332799;-8.84779627700765]
Robot Arm 1 end: [-7.31446007507306;-8.11584235746787;0.4918468852754]
Robot Arm 1 togoal: 25.0805388328376
Mobile Platform 2 Pose: [9.43671499260642;0.304971531696123;3.49252386210062]
Robot Arm 2 end: [0.661433198943391;3.54983527897799;0.281897473722852]
Robot Arm 1 togoal: 17.2858198825973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.640056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-4.19401349264359;-7.26343259472432;-8.83085318975455]
Robot Arm 1 end: [-7.46160666272574;-8.15210805740335;0.359671480892617]
Robot Arm 1 togoal: 25.2141908079694
Mobile Platform 2 Pose: [9.44724108557901;0.297084462990283;3.4934539337628]
Robot Arm 2 end: [0.65136915060698;3.56234039005544;0.287846178056876]
Robot Arm 1 togoal: 17.2889967894845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967100e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-4.39398405637421;-7.18462185456561;-8.65525614637166]
Robot Arm 1 end: [-7.24745275233268;-7.91610047874303;0.47435957531767]
Robot Arm 1 togoal: 24.8908967091775
Mobile Platform 2 Pose: [9.46625669993343;0.28376934740583;3.49462494890226]
Robot Arm 2 end: [0.610491624756674;3.54221802072166;0.285945434501987]
Robot Arm 1 togoal: 17.2481755910458

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.649569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.921473e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-4.33174907269057;-7.22320189480477;-8.625225615929]
Robot Arm 1 end: [-7.42785602687348;-7.94084562594199;0.374631349733319]
Robot Arm 1 togoal: 25.0383359780483
Mobile Platform 2 Pose: [9.48709186823768;0.268404723377876;3.49486732214924]
Robot Arm 2 end: [0.54975193258935;3.49955246377263;0.280067176404979]
Robot Arm 1 togoal: 17.1777500515637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.858926e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919589e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-4.57634587351723;-7.10746252752908;-8.5092487454953]
Robot Arm 1 end: [-6.99058511715977;-7.8070834873649;0.335816304365313]
Robot Arm 1 togoal: 24.6409590994716
Mobile Platform 2 Pose: [9.50471156680081;0.253810308036193;3.49379065323705]
Robot Arm 2 end: [0.476049727631036;3.46260220115126;0.272836050818119]
Robot Arm 1 togoal: 17.1040109087395

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.868149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.850106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-4.50569098161841;-7.14741006798384;-8.49637651755438]
Robot Arm 1 end: [-7.18666581308919;-7.73877825179874;0.533267319717122]
Robot Arm 1 togoal: 24.7188460791558
Mobile Platform 2 Pose: [9.51691638260113;0.241526261513777;3.49097542922319]
Robot Arm 2 end: [0.397102543513508;3.50231766877598;0.261910501553835]
Robot Arm 1 togoal: 17.0879381463643

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.789967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817208e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-4.45353579918676;-7.18256554014921;-8.42654794339452]
Robot Arm 1 end: [-7.31817639977914;-7.77966877663011;0.221701365172409]
Robot Arm 1 togoal: 24.8540546503585
Mobile Platform 2 Pose: [9.53685865694504;0.224218793776144;3.48272546785178]
Robot Arm 2 end: [0.311400964443155;3.42029208049371;0.262981708574831]
Robot Arm 1 togoal: 16.970911566467

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006365e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.903593e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-4.45698111177713;-7.18350158755754;-8.40748218252989]
Robot Arm 1 end: [-7.22173509045419;-7.66929817839719;0.413107888367016]
Robot Arm 1 togoal: 24.6985765293045
Mobile Platform 2 Pose: [9.54149116300237;0.217476478151478;3.47074195531698]
Robot Arm 2 end: [0.239717002780427;3.38846775278637;0.223772722458892]
Robot Arm 1 togoal: 16.9051965878551

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.932369e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-4.31131747280751;-7.27452433769285;-8.30849767660317]
Robot Arm 1 end: [-7.58116488241213;-7.62303148481691;0.588118043088441]
Robot Arm 1 togoal: 24.9106175371656
Mobile Platform 2 Pose: [9.5291569897798;0.225368062407623;3.45400863832026]
Robot Arm 2 end: [0.185935526831907;3.33389868985904;0.191926350157484]
Robot Arm 1 togoal: 16.8318915631876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003160e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-4.39782137187853;-7.228676593933;-8.29344776263082]
Robot Arm 1 end: [-7.37467254779644;-7.5820429795567;0.383837402088031]
Robot Arm 1 togoal: 24.7447089344817
Mobile Platform 2 Pose: [9.51918006801385;0.234460462411922;3.43709468154919]
Robot Arm 2 end: [0.0511527311220598;3.36253765571094;0.169455277678374]
Robot Arm 1 togoal: 16.7752721873915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.895287e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995114e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-4.57429244198848;-7.13762389947929;-8.23021468639708]
Robot Arm 1 end: [-6.92161669523929;-7.4912964003677;0.540295071352892]
Robot Arm 1 togoal: 24.3566899126952
Mobile Platform 2 Pose: [9.53698570686623;0.226049752949724;3.42522110812316]
Robot Arm 2 end: [-0.0765212924056341;3.49096644719833;0.181440384380243]
Robot Arm 1 togoal: 16.8010721474348

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.493563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-4.49703880816796;-7.17776578709779;-8.1990762592937]
Robot Arm 1 end: [-7.12764709083434;-7.44995954503951;0.494220179739266]
Robot Arm 1 togoal: 24.4726716929512
Mobile Platform 2 Pose: [9.55801876630678;0.213754080007664;3.42066634463257]
Robot Arm 2 end: [-0.139500268442967;3.64778757981536;0.228096922066406]
Robot Arm 1 togoal: 16.8867746816474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.836239e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-4.74727521915232;-7.03524426891283;-8.09371400787583]
Robot Arm 1 end: [-6.84946528070508;-7.49266420885101;0.175383728427437]
Robot Arm 1 togoal: 24.3250583662121
Mobile Platform 2 Pose: [9.55582335800808;0.214690245769701;3.41830764396189]
Robot Arm 2 end: [-0.12475720478029;3.71716074650321;0.211365406401296]
Robot Arm 1 togoal: 16.9527597157129

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.117355e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-4.76538880148361;-7.02222292500267;-8.07351900678569]
Robot Arm 1 end: [-6.72931146993488;-7.37337808881128;0.545492700983323]
Robot Arm 1 togoal: 24.138263047811
Mobile Platform 2 Pose: [9.59164448227179;0.194104905246154;3.41299897326692]
Robot Arm 2 end: [-0.0701515382271971;3.69143756706679;0.184369049396706]
Robot Arm 1 togoal: 16.965964028748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.328747e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000035e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-4.81156567870993;-6.98539206237012;-8.01944242243361]
Robot Arm 1 end: [-6.95496164900985;-7.32661995685489;0.306342581979198]
Robot Arm 1 togoal: 24.2725745707337
Mobile Platform 2 Pose: [9.61449681356523;0.180643288503002;3.40869147369869]
Robot Arm 2 end: [0.0142704912840672;3.64517561615244;0.153701363126675]
Robot Arm 1 togoal: 16.9807519453833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000568e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-4.99051607666948;-6.88335175778473;-7.97927693966795]
Robot Arm 1 end: [-6.72940181608787;-7.2427722823575;0.490604101350881]
Robot Arm 1 togoal: 24.0467835024419
Mobile Platform 2 Pose: [9.63396717156183;0.168848170997305;3.40313670579322]
Robot Arm 2 end: [0.0889780157509462;3.54829082666537;0.133731383488421]
Robot Arm 1 togoal: 16.9489874606293

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.055972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-5.22382649614966;-6.74059854350705;-7.84994461022912]
Robot Arm 1 end: [-6.69910056045889;-7.16350532951305;0.304145813809142]
Robot Arm 1 togoal: 23.9776272561526
Mobile Platform 2 Pose: [9.67075023089268;0.145503187415859;3.39598391411706]
Robot Arm 2 end: [0.139330622116176;3.46165805137457;0.15246692978352]
Robot Arm 1 togoal: 16.9084341593477

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.143062e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-5.4342760998264;-6.62016248817875;-7.80842210260221]
Robot Arm 1 end: [-6.45928102846012;-7.07542684063308;0.49510546982313]
Robot Arm 1 togoal: 23.7387993241168
Mobile Platform 2 Pose: [9.71272758309984;0.11738953330279;3.39141793402258]
Robot Arm 2 end: [0.159106088943898;3.36091093132445;0.223605203833882]
Robot Arm 1 togoal: 16.8346253506967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.568653e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-5.69857308211817;-6.46155285858727;-7.70752249991072]
Robot Arm 1 end: [-6.41178452982364;-7.02499484392241;0.313361039019953]
Robot Arm 1 togoal: 23.6781653163631
Mobile Platform 2 Pose: [9.73994709020626;0.098067634065119;3.38601500989347]
Robot Arm 2 end: [0.183669891449442;3.28685941660792;0.313003986801118]
Robot Arm 1 togoal: 16.7841300037452
Step 243:
Mobile Platform 1 Pose: [-5.96984735780186;-6.30591596344443;-7.6673538779786]
Robot Arm 1 end: [-6.13184228749694;-6.93974148906034;0.487783660218487]
Robot Arm 1 togoal: 23.4149250118308
Mobile Platform 2 Pose: [9.76802005486072;0.0763573006163906;3.37936808340919]
Robot Arm 2 end: [0.238762838706956;3.26080889097192;0.369339997709721]
Robot Arm 1 togoal: 16.7930830305528
Step 244:
Mobile Platform 1 Pose: [-6.24856044675944;-6.13968735313817;-7.58234197376636]
Robot Arm 1 end: [-6.08061766337881;-6.89726727616635;0.319195198161894]
Robot Arm 1 togoal: 23.3569741323591
Mobile Platform 2 Pose: [9.77626444468701;0.0674393801584524;3.3724835747645]
Robot Arm 2 end: [0.316658741255516;3.27371266960611;0.423001205217081]
Robot Arm 1 togoal: 16.8472697186259
Step 245:
Mobile Platform 1 Pose: [-6.50880912121631;-5.99031694133172;-7.54643793766915]
Robot Arm 1 end: [-5.80646114139845;-6.82095390691521;0.489786595979887]
Robot Arm 1 togoal: 23.105239778841
Mobile Platform 2 Pose: [9.77238367395381;0.0680270059886197;3.3688269977777]
Robot Arm 2 end: [0.370691714398745;3.3046742735372;0.449894720752203]
Robot Arm 1 togoal: 16.9030432372178
Step 246:
Mobile Platform 1 Pose: [-6.79926408302784;-5.81818029306048;-7.47404330955242]
Robot Arm 1 end: [-5.75637772228007;-6.77820619773693;0.337744152204002]
Robot Arm 1 togoal: 23.0471633079276
Mobile Platform 2 Pose: [9.77727961293626;0.0637611327868992;3.36792230058063]
Robot Arm 2 end: [0.372472217878251;3.32544280923501;0.436164829516966]
Robot Arm 1 togoal: 16.9213614499533
Step 247:
Mobile Platform 1 Pose: [-7.05679449258963;-5.67069907649576;-7.43829962794487]
Robot Arm 1 end: [-5.47695277925418;-6.71446799684642;0.491137829496715]
Robot Arm 1 togoal: 22.8028534144107
Mobile Platform 2 Pose: [9.79210467131783;0.0527455338829074;3.36749284041387]
Robot Arm 2 end: [0.336079978793813;3.33550653990004;0.392943825363799]
Robot Arm 1 togoal: 16.909843131646
Step 248:
Mobile Platform 1 Pose: [-7.29278706047065;-5.53058586139035;-7.37798986839493]
Robot Arm 1 end: [-5.47280452158304;-6.65022242919107;0.415804002547807]
Robot Arm 1 togoal: 22.7565165369712
Mobile Platform 2 Pose: [9.81363523848293;0.0367355024777116;3.36651911246895]
Robot Arm 2 end: [0.270847335136905;3.34167838131663;0.346326799079894]
Robot Arm 1 togoal: 16.8781084290018
Step 249:
Mobile Platform 1 Pose: [-7.59148346944475;-5.36056729742617;-7.3359477061542]
Robot Arm 1 end: [-5.14542971867408;-6.60725425899669;0.52605272489811]
Robot Arm 1 togoal: 22.4983300595065
Mobile Platform 2 Pose: [9.83825099624749;0.0180827616778807;3.36409683865835]
Robot Arm 2 end: [0.192181412489228;3.35101816472267;0.312572896557669]
Robot Arm 1 togoal: 16.8401467940211
Step 250:
Mobile Platform 1 Pose: [-7.68849654481136;-5.30111464564768;-7.29355389903879]
Robot Arm 1 end: [-5.25172365559743;-6.53455168972529;0.557689742393697]
Robot Arm 1 togoal: 22.5152046559501
Mobile Platform 2 Pose: [9.86386078909541;-0.00175503732731316;3.35871167177422]
Robot Arm 2 end: [0.10585492891638;3.37487928547734;0.264345716922849]
Robot Arm 1 togoal: 16.8105585788388
Step 251:
Mobile Platform 1 Pose: [-7.98561980808031;-5.13305680605428;-7.25173608789414]
Robot Arm 1 end: [-4.9159492374002;-6.55383604839516;0.356451404225158]
Robot Arm 1 togoal: 22.3130276520639
Mobile Platform 2 Pose: [9.88108511811465;-0.0160880600638125;3.34679956912757]
Robot Arm 2 end: [2.34172607418565e-05;3.39896676875678;0.213962343396687]
Robot Arm 1 togoal: 16.7702452651054
Step 252:
Mobile Platform 1 Pose: [-7.98703133241369;-5.12930051222432;-7.221251919846]
Robot Arm 1 end: [-5.07685185343144;-6.46263573632597;0.555482703232911]
Robot Arm 1 togoal: 22.3441728611205
Mobile Platform 2 Pose: [9.882295339552;-0.0176050339006549;3.33103477839601]
Robot Arm 2 end: [-0.112883950042822;3.39123755415826;0.189106594430789]
Robot Arm 1 togoal: 16.6989295184158
Step 253:
Mobile Platform 1 Pose: [-8.11850404032067;-5.05691713683114;-7.15641124788225]
Robot Arm 1 end: [-5.10721746157504;-6.4454047479779;0.372687892227355]
Robot Arm 1 togoal: 22.3606345494261
Mobile Platform 2 Pose: [9.89170613131135;-0.0222298094209724;3.32033981821921]
Robot Arm 2 end: [-0.225590380621628;3.4027856003599;0.184487056544387]
Robot Arm 1 togoal: 16.6421156138466
Step 254:
Mobile Platform 1 Pose: [-8.38719184814439;-4.90824757568048;-7.10956015351994]
Robot Arm 1 end: [-4.92437014558841;-6.35245718575996;0.538726039843594]
Robot Arm 1 togoal: 22.1608786465574
Mobile Platform 2 Pose: [9.92396707521393;-0.0409260472058978;3.31323585373742]
Robot Arm 2 end: [-0.326262110665745;3.46633492109045;0.197589860068481]
Robot Arm 1 togoal: 16.6335442812708
Step 255:
Mobile Platform 1 Pose: [-8.63836865496387;-4.77090791201478;-7.04660721190943]
Robot Arm 1 end: [-4.89724422378407;-6.34085072549085;0.382343704424505]
Robot Arm 1 togoal: 22.1415407335464
Mobile Platform 2 Pose: [9.95425918169641;-0.0585941636079202;3.30946295471988]
Robot Arm 2 end: [-0.364382768233146;3.4924652925927;0.190579903355219]
Robot Arm 1 togoal: 16.6331523393041
Step 256:
Mobile Platform 1 Pose: [-8.87429606181953;-4.64368937999306;-7.00189591281254]
Robot Arm 1 end: [-4.79968084350447;-6.25958867671062;0.547088013271704]
Robot Arm 1 togoal: 22.008027661817
Mobile Platform 2 Pose: [9.97288715697299;-0.069288322938991;3.30699698553835]
Robot Arm 2 end: [-0.368723107870976;3.48165682329186;0.178997274876246]
Robot Arm 1 togoal: 16.6228017069359
Step 257:
Mobile Platform 1 Pose: [-9.14455412143696;-4.5022294777102;-6.9398534709076]
Robot Arm 1 end: [-4.76550332293793;-6.24353364134414;0.478201239458279]
Robot Arm 1 togoal: 21.9763487138368
Mobile Platform 2 Pose: [9.98368695843551;-0.0751242806090998;3.30451710688696]
Robot Arm 2 end: [-0.353058553638512;3.47135105513169;0.175439637561208]
Robot Arm 1 togoal: 16.6238175883006
Step 258:
Mobile Platform 1 Pose: [-9.3987009982155;-4.37230097492867;-6.88695856796601]
Robot Arm 1 end: [-4.78373704929518;-6.24304226205012;0.647516378616311]
Robot Arm 1 togoal: 21.9808616006351
Mobile Platform 2 Pose: [9.9944127820521;-0.0808886893442563;3.30174237115975]
Robot Arm 2 end: [-0.322815427326374;3.44933422375777;0.17662133367914]
Robot Arm 1 togoal: 16.6234727513355
<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/plot/real_time_2D_plot.m',49,1)">49  </a>end
if system_dependent('IsDebugMode')==1, dbstep; end
if system_dependent('IsDebugMode')==1, dbstep; end
if system_dependent('IsDebugMode')==1, dbquit all; end
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.884288e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
{输入参数的数目不足。

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('update_human_models', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/human/update_human_models.m', 13)" style="font-weight:bold">update_human_models</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/System_model/human/update_human_models.m',13,0)">第 13 行</a>)
    num_frames = size(humanwalk.data, 1);

出错 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 113)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',113,0)">第 113 行</a>)
    human=update_human_models;
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.884828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.784973646531428;-9.99971132853111;-9.82820014455123]
Robot Arm 1 end: [-10.0380417347551;-9.82872531833671;1.51997973247882]
Robot Arm 1 togoal: 28.1904498962819
Mobile Platform 2 Pose: [-2.37385947600833;10.0087601541528;9.81851213443811]
Robot Arm 2 end: [9.77331405088098;9.82827264378698;1.48603581591066]
Robot Arm 1 togoal: 28.0025992915888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.858396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.739604349491108;-9.96943303111976;-9.82072865114823]
Robot Arm 1 end: [-10.1974699291175;-9.79506744008313;1.48828174146052]
Robot Arm 1 togoal: 28.2804447804395
Mobile Platform 2 Pose: [-2.33480138374786;9.98255446650136;9.81776772657159]
Robot Arm 2 end: [9.74627674820185;9.81128640555498;1.48206320944473]
Robot Arm 1 togoal: 27.9714860265443

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.886788e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.724841869533536;-9.95882392993013;-9.82238721416411]
Robot Arm 1 end: [-10.3142909452569;-9.79067783517213;1.43897637236528]
Robot Arm 1 togoal: 28.3610308990983
Mobile Platform 2 Pose: [-2.28872544341964;9.95199858002548;9.81561073522342]
Robot Arm 2 end: [9.79626185040892;9.79712118675341;1.49257035386578]
Robot Arm 1 togoal: 27.9967640769025

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.909602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.707446299756816;-9.94613443473812;-9.82400610972553]
Robot Arm 1 end: [-10.4386002883094;-9.77939556281872;1.35325178012974]
Robot Arm 1 togoal: 28.4427321128088
Mobile Platform 2 Pose: [-2.24062281874845;9.91998606267499;9.81209204727248]
Robot Arm 2 end: [9.88198222620333;9.7844466082014;1.49091598968869]
Robot Arm 1 togoal: 28.0485017404409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.841153e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.694743598083198;-9.93646434509162;-9.82539740154756]
Robot Arm 1 end: [-10.5426144886992;-9.76882813262693;1.24444191494507]
Robot Arm 1 togoal: 28.5110866180696
Mobile Platform 2 Pose: [-2.1941038482135;9.88826780150749;9.80677127334726]
Robot Arm 2 end: [9.9560649798716;9.76409677761577;1.46079331564237]
Robot Arm 1 togoal: 28.0867882777768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.896838e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.683430442629134;-9.92762963830151;-9.82667587035461]
Robot Arm 1 end: [-10.6218391832771;-9.75262622506869;1.11624106426062]
Robot Arm 1 togoal: 28.5585420475954
Mobile Platform 2 Pose: [-2.16005652464031;9.863381240583;9.79874791345942]
Robot Arm 2 end: [10.0585991082155;9.73841294667047;1.41142687836676]
Robot Arm 1 togoal: 28.141861169908

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.673660205265545;-9.91981700803529;-9.82786650745219]
Robot Arm 1 end: [-10.6702137448991;-9.73028446400646;0.981651818587247]
Robot Arm 1 togoal: 28.5802861733613
Mobile Platform 2 Pose: [-2.14173568951874;9.84680857700059;9.79029841203151]
Robot Arm 2 end: [10.2222508082255;9.69481523484479;1.33083113663396]
Robot Arm 1 togoal: 28.2287325093077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.972666e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.664976470879589;-9.91281690344043;-9.82897491150644]
Robot Arm 1 end: [-10.6886690918901;-9.70222317386696;0.851335210351745]
Robot Arm 1 togoal: 28.5770375951931
Mobile Platform 2 Pose: [-2.13341005285513;9.83311770936138;9.78173822158482]
Robot Arm 2 end: [10.3447015008929;9.58480584980768;1.21024732127506]
Robot Arm 1 togoal: 28.2412476098778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.656833109084103;-9.90634481105832;-9.82999922198105]
Robot Arm 1 end: [-10.6818191461944;-9.67022721129422;0.732643499736038]
Robot Arm 1 togoal: 28.5530556632971
Mobile Platform 2 Pose: [-2.14944157406559;9.83087780920209;9.77257830820982]
Robot Arm 2 end: [10.4118245640898;9.43551040244897;1.07592328312124]
Robot Arm 1 togoal: 28.1882908301087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979390e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.648766320798012;-9.90013952529218;-9.83093799505939]
Robot Arm 1 end: [-10.6558390361124;-9.63686825400093;0.629177471239773]
Robot Arm 1 togoal: 28.5143056821234
Mobile Platform 2 Pose: [-2.1882286234948;9.84686859140381;9.76563017339956]
Robot Arm 2 end: [10.3747966375623;9.30584098440644;1.0639848042692]
Robot Arm 1 togoal: 28.0723475510392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987684e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.640436171253537;-9.89398799753507;-9.83179537519992]
Robot Arm 1 end: [-10.6167964117317;-9.60472993248833;0.5410851631173]
Robot Arm 1 togoal: 28.4667870174374
Mobile Platform 2 Pose: [-2.25044130176446;9.87837457533849;9.7554007267324]
Robot Arm 2 end: [10.2774355049595;9.23814770057402;1.15840281591937]
Robot Arm 1 togoal: 27.9537509884594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.631539882607877;-9.88767584010998;-9.83258627602768]
Robot Arm 1 end: [-10.5839217542558;-9.57457291186147;0.465698167471456]
Robot Arm 1 togoal: 28.4249081163835
Mobile Platform 2 Pose: [-2.36504327629258;9.93748934077468;9.72902201996442]
Robot Arm 2 end: [10.3226457258492;9.31922483323327;1.26574348405925]
Robot Arm 1 togoal: 28.0411756769278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.624049755723741;-9.88227554986445;-9.83344189124978]
Robot Arm 1 end: [-10.5571910030627;-9.55981423057153;0.412113953237322]
Robot Arm 1 togoal: 28.3974305692536
Mobile Platform 2 Pose: [-2.50303754884666;10.0097815569586;9.69885043039386]
Robot Arm 2 end: [10.4546791348794;9.3923440891625;1.27700779460851]
Robot Arm 1 togoal: 28.1871680021182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979892e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.617432180399294;-9.87748918612576;-9.83427289788956]
Robot Arm 1 end: [-10.5360306324131;-9.55296683313609;0.374411923677593]
Robot Arm 1 togoal: 28.378895861712
Mobile Platform 2 Pose: [-2.61724367955336;10.0717471566958;9.67968952024131]
Robot Arm 2 end: [10.6758275008003;9.41801451301165;1.14411591333605]
Robot Arm 1 togoal: 28.3670790192639

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.611308743324291;-9.87307847110079;-9.83505465661143]
Robot Arm 1 end: [-10.5192517036697;-9.54975790490078;0.347581315773475]
Robot Arm 1 togoal: 28.3656418229391
Mobile Platform 2 Pose: [-2.62873164174053;10.0779394936833;9.67895072082341]
Robot Arm 2 end: [10.7443438685753;9.43796680225876;1.06851227510784]
Robot Arm 1 togoal: 28.4317973593754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.605501272721076;-9.86892306342162;-9.83578374406352]
Robot Arm 1 end: [-10.505892035287;-9.54821757399887;0.328693151828132]
Robot Arm 1 togoal: 28.3557051420599
Mobile Platform 2 Pose: [-2.58696118269162;10.0521695032601;9.68195834197562]
Robot Arm 2 end: [10.7679436447891;9.41335893156522;0.953594251902892]
Robot Arm 1 togoal: 28.4342739932268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995174e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.599917677615897;-9.86495791241877;-9.83646122170501]
Robot Arm 1 end: [-10.4952618442746;-9.54739525667926;0.315989086874684]
Robot Arm 1 togoal: 28.3479888296679
Mobile Platform 2 Pose: [-2.53159414882159;10.0161701990619;9.68508348213958]
Robot Arm 2 end: [10.7160422242697;9.2777418117825;0.796656933992323]
Robot Arm 1 togoal: 28.3074267229776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997478e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.594503023034803;-9.86114329093687;-9.83708888821633]
Robot Arm 1 end: [-10.4868456191286;-9.54683212722049;0.308332523642406]
Robot Arm 1 togoal: 28.3418427777723
Mobile Platform 2 Pose: [-2.50215634042233;9.99620868801031;9.68678850829072]
Robot Arm 2 end: [10.6370995047084;9.16797830262277;0.636394841244939]
Robot Arm 1 togoal: 28.1794924451089

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987675e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.589218891603615;-9.85745123633009;-9.83766864450077]
Robot Arm 1 end: [-10.4802206092727;-9.54632703470604;0.304914065612517]
Robot Arm 1 togoal: 28.3368523614034
Mobile Platform 2 Pose: [-2.45930236169752;9.97141693198324;9.68760574133361]
Robot Arm 2 end: [10.5560958267681;9.13085990921284;0.471256433853583]
Robot Arm 1 togoal: 28.1006198874663

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000009e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.58403458404484;-9.85385943447178;-9.83820251649745]
Robot Arm 1 end: [-10.4750087226887;-9.5458213855304;0.305096443778486]
Robot Arm 1 togoal: 28.3327290829399
Mobile Platform 2 Pose: [-2.45007112074761;9.9634977632969;9.68791877037844]
Robot Arm 2 end: [10.4444131196397;9.09682175056609;0.319800983769655]
Robot Arm 1 togoal: 28.0018840054733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.578923225113092;-9.85034837833736;-9.83869275513583]
Robot Arm 1 end: [-10.4708526571268;-9.54533586613511;0.308331663315341]
Robot Arm 1 togoal: 28.329252145523
Mobile Platform 2 Pose: [-2.39190133396175;9.93177912142467;9.6880040201569]
Robot Arm 2 end: [10.3184181289526;9.12175208535656;0.177065803382616]
Robot Arm 1 togoal: 27.9335940616603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000071e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.573859868663879;-9.84689993977582;-9.83914189855625]
Robot Arm 1 end: [-10.4701792521911;-9.54329053133218;0.320965044321749]
Robot Arm 1 togoal: 28.3268163990192
Mobile Platform 2 Pose: [-2.38680176420295;9.92673343648809;9.6880289980124]
Robot Arm 2 end: [10.2432068142441;9.12813065784128;0.11482936418705]
Robot Arm 1 togoal: 27.886378761767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995704e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.568744357325611;-9.84344509223267;-9.83955967828979]
Robot Arm 1 end: [-10.4703292593784;-9.53918755130075;0.339347362866552]
Robot Arm 1 togoal: 28.32332136433
Mobile Platform 2 Pose: [-2.38345504265546;9.92282148108051;9.68807186505935]
Robot Arm 2 end: [10.1825361238827;9.14474316243373;0.0636992427426805]
Robot Arm 1 togoal: 27.856411840804

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.014770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.563578085693075;-9.83999685095606;-9.83993285149266]
Robot Arm 1 end: [-10.4689997439972;-9.53418028432365;0.358263948079237]
Robot Arm 1 togoal: 28.3181232610258
Mobile Platform 2 Pose: [-2.38026060706937;9.91916119495894;9.68812238773043]
Robot Arm 2 end: [10.1374156444033;9.15550859657728;0.0323543925434066]
Robot Arm 1 togoal: 27.8328058983208

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.849313e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.558427860876471;-9.83661029535697;-9.8402448665039]
Robot Arm 1 end: [-10.4668079959747;-9.52948408373581;0.377891202822517]
Robot Arm 1 togoal: 28.3125003460163
Mobile Platform 2 Pose: [-2.3721113413444;9.91315973486976;9.68815890188332]
Robot Arm 2 end: [10.1122848703967;9.07233301343951;0.103535926634124]
Robot Arm 1 togoal: 27.7536861278426

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.920382e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.553305003392368;-9.83329403048955;-9.84049291258899]
Robot Arm 1 end: [-10.4639892150354;-9.52574559539454;0.398232078788745]
Robot Arm 1 togoal: 28.307069849316
Mobile Platform 2 Pose: [-2.33345598180584;9.89220024343303;9.68818017204702]
Robot Arm 2 end: [10.0344497646928;9.08015117134551;0.0720670799774307]
Robot Arm 1 togoal: 27.7043453758092

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.548186532567093;-9.83003037802041;-9.8406801886397]
Robot Arm 1 end: [-10.4605013346584;-9.52313382136095;0.419200966203105]
Robot Arm 1 togoal: 28.3019227294402
Mobile Platform 2 Pose: [-2.29412210929115;9.87105881366815;9.68818170895091]
Robot Arm 2 end: [9.99210644157436;9.09891184618823;0.0429251854299049]
Robot Arm 1 togoal: 27.6882187732829

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.543030359830344;-9.82678962848902;-9.84081039152489]
Robot Arm 1 end: [-10.4561168634111;-9.52147072313214;0.440695753547439]
Robot Arm 1 togoal: 28.2967770883637
Mobile Platform 2 Pose: [-2.25507572365812;9.85000222647837;9.68810028847574]
Robot Arm 2 end: [9.95951527161638;9.10736644436861;0.0269992630048447]
Robot Arm 1 togoal: 27.6713884819576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.53778014194434;-9.82353605342773;-9.84088407669266]
Robot Arm 1 end: [-10.4504833005712;-9.52035088076928;0.462653944448298]
Robot Arm 1 togoal: 28.2911021720362
Mobile Platform 2 Pose: [-2.25110705172763;9.84631975645905;9.68786850450702]
Robot Arm 2 end: [9.93501411289472;9.12220408106458;0.00530853635735412]
Robot Arm 1 togoal: 27.665154928396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.532361100518938;-9.82022742038682;-9.84089592893575]
Robot Arm 1 end: [-10.4431455940323;-9.51923731122839;0.485099827589931]
Robot Arm 1 togoal: 28.2841990545614
Mobile Platform 2 Pose: [-2.24618949607835;9.84211520310645;9.68753412376481]
Robot Arm 2 end: [9.91336753063597;9.12299662561784;-0.00274886738075136]
Robot Arm 1 togoal: 27.6505518383345

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.526667979254746;-9.81680978137528;-9.8408316411068]
Robot Arm 1 end: [-10.4335309257523;-9.51754834825859;0.508195265741002]
Robot Arm 1 togoal: 28.2752480634569
Mobile Platform 2 Pose: [-2.24103629306475;9.8377321350528;9.68706992657242]
Robot Arm 2 end: [9.89259319289096;9.11021633912679;0.00176638206967245]
Robot Arm 1 togoal: 27.6265036378462

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.520544131835174;-9.81320812628348;-9.84066260369877]
Robot Arm 1 end: [-10.4208981939122;-9.51477718641221;0.532311516304235]
Robot Arm 1 togoal: 28.2633528902576
Mobile Platform 2 Pose: [-2.23596166785097;9.83333700643356;9.68646118331768]
Robot Arm 2 end: [9.87237331281959;9.08164449498226;0.0227266331438259]
Robot Arm 1 togoal: 27.5910347902862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.513748627192006;-9.80931284062521;-9.84033525389691]
Robot Arm 1 end: [-10.4042623176981;-9.51074065581038;0.558146269388513]
Robot Arm 1 togoal: 28.2476549287597
Mobile Platform 2 Pose: [-2.22896131918246;9.8279769012751;9.68572256018527]
Robot Arm 2 end: [9.84368686143789;8.99576446984481;0.115137364018158]
Robot Arm 1 togoal: 27.5061343303611

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.505911883593327;-9.80496556319365;-9.83974872637341]
Robot Arm 1 end: [-10.3823606077463;-9.50621866686771;0.586906783004054]
Robot Arm 1 togoal: 28.2277473554285
Mobile Platform 2 Pose: [-2.22669116154037;9.82503180615485;9.68496381735952]
Robot Arm 2 end: [9.82957039455559;8.98691096624601;0.124506951497207]
Robot Arm 1 togoal: 27.4893587212817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.827959e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.496477663217229;-9.79995189200321;-9.83871115483099]
Robot Arm 1 end: [-10.3313514860506;-9.46386956885923;0.62040281699795]
Robot Arm 1 togoal: 28.1605601352127
Mobile Platform 2 Pose: [-2.22433274922985;9.82235771160735;9.68425926320464]
Robot Arm 2 end: [9.82269295293817;8.98030438616924;0.133732004273364]
Robot Arm 1 togoal: 27.4793672232471

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.957571e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.548995504089013;-9.82587391538731;-9.83813601008041]
Robot Arm 1 end: [-10.2279454284598;-9.292609114515;0.592912346682014]
Robot Arm 1 togoal: 27.9684802839635
Mobile Platform 2 Pose: [-2.22198725324109;9.82003430632765;9.68364984940374]
Robot Arm 2 end: [9.8217133777219;8.97586259244185;0.142222149642193]
Robot Arm 1 togoal: 27.4751653446605

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.785588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.5057423949141;-9.80039466449035;-9.83738771509966]
Robot Arm 1 end: [-10.2959829694997;-9.35677581910419;0.632317654739552]
Robot Arm 1 togoal: 28.0606744594647
Mobile Platform 2 Pose: [-2.22016788849754;9.81825248778735;9.68313144469771]
Robot Arm 2 end: [9.82461519438057;8.97305224945879;0.149966022051929]
Robot Arm 1 togoal: 27.4749306356658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.722574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.901458e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.472063267647332;-9.78027707371783;-9.83646265641758]
Robot Arm 1 end: [-10.3271511105616;-9.39881321998597;0.656700661839819]
Robot Arm 1 togoal: 28.1114574499696
Mobile Platform 2 Pose: [-2.24455648011815;9.82976980479074;9.68268226068755]
Robot Arm 2 end: [9.6322286321762;8.97166252678346;0.174546095714866]
Robot Arm 1 togoal: 27.3342517404915

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.790406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998162e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.44166572087613;-9.76209254876181;-9.83535995941912]
Robot Arm 1 end: [-10.346350688847;-9.43388441935878;0.672781054479205]
Robot Arm 1 togoal: 28.1490554967317
Mobile Platform 2 Pose: [-2.24027311731444;9.82667673477808;9.68209817983877]
Robot Arm 2 end: [9.43158728469838;9.02841232289613;0.210452352456629]
Robot Arm 1 togoal: 27.2283784877006

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.829640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990266e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.413659317704091;-9.74537414937712;-9.83405092244089]
Robot Arm 1 end: [-10.3563816681207;-9.45984691325404;0.682388599715927]
Robot Arm 1 togoal: 28.1739451548087
Mobile Platform 2 Pose: [-2.23850002471278;9.82658532294547;9.68201641144375]
Robot Arm 2 end: [9.39225286767896;9.05056519829023;0.212368247509415]
Robot Arm 1 togoal: 27.215731162729

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.853517e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.387661824826467;-9.72993465989175;-9.83253565802129]
Robot Arm 1 end: [-10.3586044166409;-9.47479343245345;0.687770713769194]
Robot Arm 1 togoal: 28.1857166677279
Mobile Platform 2 Pose: [-2.23520405987157;9.82636199501467;9.68180660416115]
Robot Arm 2 end: [9.40885784551865;9.04399066734491;0.202495820531842]
Robot Arm 1 togoal: 27.22344204315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.868695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.363248658141096;-9.71553771750377;-9.83083426416718]
Robot Arm 1 end: [-10.3541925312894;-9.47851636802858;0.691197839661324]
Robot Arm 1 togoal: 28.1850017762224
Mobile Platform 2 Pose: [-2.23138807862363;9.82572952420841;9.68143062510431]
Robot Arm 2 end: [9.41707616516224;9.02746399865739;0.216369826448032]
Robot Arm 1 togoal: 27.2170772294465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.878386e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.339732238652984;-9.70177672420713;-9.82896399742117]
Robot Arm 1 end: [-10.3445777264172;-9.47202275154225;0.694944113608913]
Robot Arm 1 togoal: 28.173460583149
Mobile Platform 2 Pose: [-2.22654149396047;9.82454663512009;9.68084012610715]
Robot Arm 2 end: [9.41772180462454;9.00003659206335;0.257708068431418]
Robot Arm 1 togoal: 27.1964191960248

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.884418e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.316870151619455;-9.68851126147985;-9.82693083917644]
Robot Arm 1 end: [-10.3297054382925;-9.45609248554338;0.702088635764567]
Robot Arm 1 togoal: 28.1515021594982
Mobile Platform 2 Pose: [-2.22024709964771;9.82262421462125;9.67998153515821]
Robot Arm 2 end: [9.41505099072655;8.96473053931633;0.327935907606587]
Robot Arm 1 togoal: 27.1666820081133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.888297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.294034527568493;-9.67535114782462;-9.82475426629363]
Robot Arm 1 end: [-10.3096397382871;-9.43295103091877;0.714535084564954]
Robot Arm 1 togoal: 28.120658557881
Mobile Platform 2 Pose: [-2.21209533647369;9.81975785505467;9.67878314248664]
Robot Arm 2 end: [9.41616930195557;8.92683196815962;0.428514572535037]
Robot Arm 1 togoal: 27.1368038486918

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.891116e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.270477749751708;-9.66181731569363;-9.82247455883053]
Robot Arm 1 end: [-10.2845798667391;-9.40547209936426;0.732838112641522]
Robot Arm 1 togoal: 28.083050527498
Mobile Platform 2 Pose: [-2.20152913026719;9.81568603955572;9.67710629537886]
Robot Arm 2 end: [9.43074667523526;8.8937601558185;0.56230288929692]
Robot Arm 1 togoal: 27.1191295828762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.893203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.245710660787057;-9.64756584558276;-9.82016682102339]
Robot Arm 1 end: [-10.2543987274606;-9.3754918741559;0.755877631963689]
Robot Arm 1 togoal: 28.0398959594095
Mobile Platform 2 Pose: [-2.18755074593015;9.80995686799706;9.67461554482053]
Robot Arm 2 end: [9.47191970860706;8.87593278986161;0.71615689549227]
Robot Arm 1 togoal: 27.1312415537639

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.902090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.218851493816947;-9.63208347202827;-9.81793266141325]
Robot Arm 1 end: [-10.2265432463118;-9.35564182488204;0.785749230392516]
Robot Arm 1 togoal: 28.0052587176454
Mobile Platform 2 Pose: [-2.16826485992144;9.80172104700557;9.67039136540488]
Robot Arm 2 end: [9.55526113574135;8.87634607947139;0.862806244861644]
Robot Arm 1 togoal: 27.1874584766269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917424e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.19026707232439;-9.61559661066721;-9.81586982959303]
Robot Arm 1 end: [-10.2085797162951;-9.35536033650614;0.822242042752537]
Robot Arm 1 togoal: 27.9911580787338
Mobile Platform 2 Pose: [-2.13939642956788;9.78919466953594;9.66104780839021]
Robot Arm 2 end: [9.69206147519827;8.90160162948626;0.993567625771353]
Robot Arm 1 togoal: 27.3006595929873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.920017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.162774118329446;-9.59962946026649;-9.81398257124119]
Robot Arm 1 end: [-10.1858022533635;-9.34834738226458;0.852786162800389]
Robot Arm 1 togoal: 27.9691199140928
Mobile Platform 2 Pose: [-2.08926518071713;9.76981247021385;9.61710279573842]
Robot Arm 2 end: [9.8232905380404;8.94076408058282;1.10447609735218]
Robot Arm 1 togoal: 27.4205923326491

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.921802e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000244e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.136145388092755;-9.58405857628045;-9.81226391739857]
Robot Arm 1 end: [-10.1580627012858;-9.3349009464132;0.879453230161256]
Robot Arm 1 togoal: 27.939174446187
Mobile Platform 2 Pose: [-2.13080316899501;9.79490974744595;9.55954404525332]
Robot Arm 2 end: [9.94217441718827;8.73396797928002;0.726314869259626]
Robot Arm 1 togoal: 27.3730415042112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.110969888582949;-9.56917055596153;-9.8107399341372]
Robot Arm 1 end: [-10.11776258915;-9.30421249463114;0.898124618044574]
Robot Arm 1 togoal: 27.8884378438937
Mobile Platform 2 Pose: [-2.15273717233706;9.80766213571784;9.54521136841699]
Robot Arm 2 end: [9.80058653177804;8.74178398353812;0.871878064829747]
Robot Arm 1 togoal: 27.2715795870858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.910975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001222e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.0874330634823385;-9.55504157577265;-9.80950210339627]
Robot Arm 1 end: [-10.0597346502519;-9.25487504913708;0.907990150816003]
Robot Arm 1 togoal: 27.8122086226821
Mobile Platform 2 Pose: [-2.13473041330707;9.80852930296496;9.48563619116004]
Robot Arm 2 end: [9.93705929993643;8.835032879872;1.11243832567699]
Robot Arm 1 togoal: 27.4301243117265

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.903514e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.0664116153073712;-9.54215749716878;-9.80869607215194]
Robot Arm 1 end: [-9.97728538633755;-9.18854960122042;0.90626088764911]
Robot Arm 1 togoal: 27.7068632541096
Mobile Platform 2 Pose: [-2.16727864505413;9.82989494013219;9.4616017694316]
Robot Arm 2 end: [9.95692022085012;8.71813922693834;1.00121851224651]
Robot Arm 1 togoal: 27.3663584281036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.902437e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.0490096934853428;-9.53122762811884;-9.80844615700436]
Robot Arm 1 end: [-9.87315723591193;-9.11815036339947;0.890889783577285]
Robot Arm 1 togoal: 27.5833843232782
Mobile Platform 2 Pose: [-2.27582817592061;9.8946635292959;9.38247685670179]
Robot Arm 2 end: [9.81527969754579;8.67841261248783;1.10180910486809]
Robot Arm 1 togoal: 27.2342455048947

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.926778e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.0346765873877028;-9.52216280218999;-9.80870085719633]
Robot Arm 1 end: [-9.76673300385075;-9.05825913109564;0.85506041662838]
Robot Arm 1 togoal: 27.4660599701197
Mobile Platform 2 Pose: [-2.22508766240278;9.87784326647923;9.3279684972974]
Robot Arm 2 end: [9.97330614220153;8.72073309857394;1.19659924976314]
Robot Arm 1 togoal: 27.3770714621725

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.0241929694033377;-9.51534822226157;-9.80926107753025]
Robot Arm 1 end: [-9.64222394687699;-9.00554733788088;0.78759329086753]
Robot Arm 1 togoal: 27.3416472911326
Mobile Platform 2 Pose: [-2.25914085004972;9.89624925404005;9.30101176137662]
Robot Arm 2 end: [9.84196261276809;8.59169884280587;1.10114244289799]
Robot Arm 1 togoal: 27.1943197479976

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997711e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.0166953637451999;-9.51033940930669;-9.81001609934328]
Robot Arm 1 end: [-9.51292333520792;-8.9763300673503;0.704585136968753]
Robot Arm 1 togoal: 27.2308981331741
Mobile Platform 2 Pose: [-2.21346195820745;9.87342007974271;9.14468270234731]
Robot Arm 2 end: [9.76369115054625;8.53649281011686;1.19312896481245]
Robot Arm 1 togoal: 27.0982020254003

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.849164e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.00147595575167046;-9.50172159190086;-9.8107830843082]
Robot Arm 1 end: [-9.3474309790301;-8.98156248171205;0.638562882572917]
Robot Arm 1 togoal: 27.1182870956424
Mobile Platform 2 Pose: [-2.17716630454295;9.85543185500198;9.10697560456046]
Robot Arm 2 end: [9.89241365165757;8.39337168599506;1.0627770008082]
Robot Arm 1 togoal: 27.0967564698214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.868803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-0.00739943339779947;-9.49653992619818;-9.81133010012986]
Robot Arm 1 end: [-9.18775323215731;-9.01985339005116;0.627197286082449]
Robot Arm 1 togoal: 27.0318993111585
Mobile Platform 2 Pose: [-2.23988123050105;9.88820372513137;9.01419816672337]
Robot Arm 2 end: [9.71925231650268;8.24507722632445;0.961961016554287]
Robot Arm 1 togoal: 26.8708816286599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.865296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-0.00688029070880964;-9.4962098372303;-9.8115338282394]
Robot Arm 1 end: [-9.04057932077247;-9.09594330063803;0.557780709631973]
Robot Arm 1 togoal: 26.9838192777292
Mobile Platform 2 Pose: [-2.19860824898646;9.87077722062177;8.99379363437541]
Robot Arm 2 end: [9.86025586055058;8.28643249195996;1.07649036649912]
Robot Arm 1 togoal: 27.0003882369091

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-0.00325981245405788;-9.49736002671935;-9.81130263860166]
Robot Arm 1 end: [-8.91085566371773;-9.18404502744041;0.636113077220489]
Robot Arm 1 togoal: 26.9523852453717
Mobile Platform 2 Pose: [-2.19227989992851;9.87725412574667;8.90785952721507]
Robot Arm 2 end: [10.0259821997819;8.1540488058775;0.92535053020381]
Robot Arm 1 togoal: 27.0363505193107

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.938008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.00586261990188985;-9.50114167705539;-9.81076426104775]
Robot Arm 1 end: [-8.88016473793198;-9.21257275861512;0.707436014884138]
Robot Arm 1 togoal: 26.9489417026793
Mobile Platform 2 Pose: [-2.28468248419176;9.92642094249658;8.85685835261176]
Robot Arm 2 end: [9.87408267618787;8.04058629642212;0.830002293972037]
Robot Arm 1 togoal: 26.8499421594971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.895017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.0194895571815301;-9.50685481626947;-9.80993712781057]
Robot Arm 1 end: [-8.83690342486257;-9.26649385051835;0.720371582922242]
Robot Arm 1 togoal: 26.956775062902
Mobile Platform 2 Pose: [-2.25917654831615;9.91920902231835;8.8297737617768]
Robot Arm 2 end: [10.025871975549;8.08092742464966;0.954791354699303]
Robot Arm 1 togoal: 26.9865906914256

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.00945653262512274;-9.50006552574746;-9.80853112187468]
Robot Arm 1 end: [-8.86425087270557;-9.25998266663999;0.846484534300563]
Robot Arm 1 togoal: 26.9677930951184
Mobile Platform 2 Pose: [-2.36074868151674;9.99026516373363;8.72625682440537]
Robot Arm 2 end: [10.2747574231049;7.97118216912957;0.802986534302969]
Robot Arm 1 togoal: 27.1024855033392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-0.0130934046493931;-9.48538633111543;-9.80612015754779]
Robot Arm 1 end: [-8.91261583511973;-9.25824379520645;0.961974667709213]
Robot Arm 1 togoal: 26.9977684471249
Mobile Platform 2 Pose: [-2.42292184661809;10.025328161028;8.70545246635264]
Robot Arm 2 end: [10.1014282604449;7.94976337151676;0.97003651613079]
Robot Arm 1 togoal: 26.9548693924459

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821620e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-0.0816347670320475;-9.44231572842848;-9.80080726639493]
Robot Arm 1 end: [-8.95319482574458;-9.2906638032066;1.10705257555152]
Robot Arm 1 togoal: 27.0467124904522
Mobile Platform 2 Pose: [-2.50797232840489;10.0914993831709;8.57942330247347]
Robot Arm 2 end: [10.3594150614926;7.87487609071458;0.933978039178935]
Robot Arm 1 togoal: 27.0991064739599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.866342e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-0.250040877023994;-9.33729268323676;-9.78878135519007]
Robot Arm 1 end: [-8.85523312417025;-9.42545420506501;1.22580296354761]
Robot Arm 1 togoal: 27.0731458927516
Mobile Platform 2 Pose: [-2.57584254421501;10.1278843007569;8.55738315187126]
Robot Arm 2 end: [10.1903894075396;7.74901118000711;0.816974222168819]
Robot Arm 1 togoal: 26.8919109509153

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971287e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.914730e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-0.49712160269441;-9.17439499007403;-9.76422835218305]
Robot Arm 1 end: [-8.58906477242582;-9.42302142028146;1.14743873610053]
Robot Arm 1 togoal: 26.8876670282872
Mobile Platform 2 Pose: [-2.60624464459304;10.1564606875196;8.48940205497866]
Robot Arm 2 end: [10.3715016568583;7.76585902667773;0.930495945645604]
Robot Arm 1 togoal: 27.0365035045099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001073e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007181e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-0.760120274724063;-8.98346556874599;-9.71863031181406]
Robot Arm 1 end: [-8.5101055583723;-9.11350283594549;1.0099253769581]
Robot Arm 1 togoal: 26.6122377058394
Mobile Platform 2 Pose: [-2.69353902243033;10.2172280508732;8.442485930765]
Robot Arm 2 end: [10.581970316269;7.72304101958346;0.791898283179358]
Robot Arm 1 togoal: 27.1708533983584

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.881666e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.953637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-0.879769201799274;-8.91142332941459;-9.71263525521482]
Robot Arm 1 end: [-8.40287843111661;-9.23090520222616;1.09426698033161]
Robot Arm 1 togoal: 26.6209644926239
Mobile Platform 2 Pose: [-2.75147318064467;10.2514602531618;8.42612940058295]
Robot Arm 2 end: [10.452030315692;7.62568272819018;0.666537123350104]
Robot Arm 1 togoal: 27.0125959966157

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.830635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943065e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-1.08624497347808;-8.78147487219734;-9.697040797019]
Robot Arm 1 end: [-8.14153221576818;-9.28679385425744;0.957284665576704]
Robot Arm 1 togoal: 26.484210074885
Mobile Platform 2 Pose: [-2.77241640584869;10.2721097095875;8.40424611755461]
Robot Arm 2 end: [10.4939995589114;7.65249284576733;0.817340979114531]
Robot Arm 1 togoal: 27.0575249719593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002650e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-1.43752177666676;-8.53980827917187;-9.65078159277236]
Robot Arm 1 end: [-7.94551891441064;-9.09882810641198;0.890279269232708]
Robot Arm 1 togoal: 26.2145893117928
Mobile Platform 2 Pose: [-2.85340949514549;10.3337293893211;8.35426537732368]
Robot Arm 2 end: [10.7275710418311;7.65035621892925;0.659714691974623]
Robot Arm 1 togoal: 27.2380081564543

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.981434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-1.58845001638184;-8.44576704817722;-9.64009682747733]
Robot Arm 1 end: [-7.9662223986842;-9.13105701548723;1.08429252219495]
Robot Arm 1 togoal: 26.2482990540335
Mobile Platform 2 Pose: [-2.91352751184644;10.3700841669685;8.33762120545645]
Robot Arm 2 end: [10.5788980748225;7.55458594590238;0.598486416525178]
Robot Arm 1 togoal: 27.0648400146334

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.866614e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.923101e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-1.78448509620653;-8.32052042730242;-9.61828339037988]
Robot Arm 1 end: [-7.68963118662335;-9.16364598008432;0.878961998853265]
Robot Arm 1 togoal: 26.0879395806506
Mobile Platform 2 Pose: [-2.95172637250894;10.4040084335211;8.30458755659063]
Robot Arm 2 end: [10.6098333251613;7.54995448098048;0.73904015670896]
Robot Arm 1 togoal: 27.0809452045518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000789e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.959469e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-2.13436739445051;-8.07819926805991;-9.55679797321271]
Robot Arm 1 end: [-7.40482792408397;-9.10569993200556;0.801267190018755]
Robot Arm 1 togoal: 25.8548606702216
Mobile Platform 2 Pose: [-3.03112584450171;10.4589225737456;8.26465690283015]
Robot Arm 2 end: [10.5276309284612;7.47249821974417;0.599905319026836]
Robot Arm 1 togoal: 26.9725831352023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001353e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.922122e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-2.39918510110763;-7.89587960697942;-9.51756605210348]
Robot Arm 1 end: [-7.4384105473094;-9.04668057767197;1.12587355663997]
Robot Arm 1 togoal: 25.8269302928285
Mobile Platform 2 Pose: [-3.13634597284765;10.5303728875043;8.13422112048254]
Robot Arm 2 end: [10.5071545634876;7.36441398593606;0.7295192669711]
Robot Arm 1 togoal: 26.8829444651529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004019e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.950286e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-2.54561704209726;-7.80138416954505;-9.49731790733774]
Robot Arm 1 end: [-7.16571051439;-9.06765060517213;0.937341914858172]
Robot Arm 1 togoal: 25.6627662933112
Mobile Platform 2 Pose: [-3.14153815148979;10.5344093985534;8.09661409733873]
Robot Arm 2 end: [10.6221992307535;7.3515773685241;0.406413673398782]
Robot Arm 1 togoal: 26.9739622778748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000647e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007925e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-2.89082745252431;-7.55899221220094;-9.43611219444547]
Robot Arm 1 end: [-6.96368093216086;-8.86774900640503;0.834778297740011]
Robot Arm 1 togoal: 25.3816729858674
Mobile Platform 2 Pose: [-3.21025776573692;10.5672720126708;8.05156052688843]
Robot Arm 2 end: [10.4570417496475;7.28091982827831;0.57375552914791]
Robot Arm 1 togoal: 26.7958582498446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000941e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.516043e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-3.0386034164079;-7.46866753781244;-9.42626083118931]
Robot Arm 1 end: [-6.85124465168263;-9.01253400324243;1.00046115980412]
Robot Arm 1 togoal: 25.4108609893381
Mobile Platform 2 Pose: [-3.16300917652263;10.5429015839743;7.98454996986087]
Robot Arm 2 end: [10.657775510556;7.2332605830613;0.564456766121278]
Robot Arm 1 togoal: 26.9192112500813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001988e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003803e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-3.35684361514843;-7.25765093637366;-9.39097028017174]
Robot Arm 1 end: [-6.50516015591161;-9.08125871483703;0.790369736848676]
Robot Arm 1 togoal: 25.2398210472315
Mobile Platform 2 Pose: [-3.30941895492078;10.601061719316;7.92383743068733]
Robot Arm 2 end: [10.5167419013649;7.1732097039901;0.437374268172867]
Robot Arm 1 togoal: 26.7774172487027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001418e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.685574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-3.53062338536323;-7.1343076644934;-9.36746555943961]
Robot Arm 1 end: [-6.52562199709834;-8.93529328728951;0.994659150104937]
Robot Arm 1 togoal: 25.1379901534892
Mobile Platform 2 Pose: [-3.32457456969936;10.5899654612716;7.84051698095676]
Robot Arm 2 end: [10.3995438408808;7.09120076929742;0.633109218476972]
Robot Arm 1 togoal: 26.6277840364602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002972e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.602177e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-3.84432985876548;-6.93124603003814;-9.33580174452857]
Robot Arm 1 end: [-6.1957523129248;-9.00690994432014;0.801215010335771]
Robot Arm 1 togoal: 24.981655450439
Mobile Platform 2 Pose: [-3.28188830399792;10.5629563836212;7.80420295889692]
Robot Arm 2 end: [10.6224965678312;7.0615633938175;0.473960807215954]
Robot Arm 1 togoal: 26.7858283650644

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002082e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.883958e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-4.06730209247653;-6.77168986475753;-9.30235838136397]
Robot Arm 1 end: [-6.23031138619368;-8.79839187891861;1.0156515844128]
Robot Arm 1 togoal: 24.8406505942847
Mobile Platform 2 Pose: [-3.52310219235139;10.646981219443;7.67680065441359]
Robot Arm 2 end: [10.4021356563058;6.98899368918072;0.355794484645886]
Robot Arm 1 togoal: 26.5750023481154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003925e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951951e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-4.31236605471612;-6.61778883700605;-9.27804637160497]
Robot Arm 1 end: [-5.99811787967022;-8.88468954150429;0.991116015360938]
Robot Arm 1 togoal: 24.7558488403852
Mobile Platform 2 Pose: [-3.48213462515046;10.6266779640986;7.65869412835699]
Robot Arm 2 end: [10.5353745057929;6.91661084177938;0.493500551503468]
Robot Arm 1 togoal: 26.6257222170975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005003e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002164e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-4.51513188254855;-6.46892730211324;-9.23069164358276]
Robot Arm 1 end: [-5.86566241749124;-8.68209344773547;0.791400031187546]
Robot Arm 1 togoal: 24.5208518540895
Mobile Platform 2 Pose: [-3.49321709258239;10.6111914872841;7.56758986845087]
Robot Arm 2 end: [10.7040393772604;6.87566019405334;0.332103683796469]
Robot Arm 1 togoal: 26.7368402764984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004978e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999616e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-4.73096994794827;-6.32570764597413;-9.19464815257932]
Robot Arm 1 end: [-5.88883670923481;-8.60610007841503;0.993192542768639]
Robot Arm 1 togoal: 24.4728558368784
Mobile Platform 2 Pose: [-3.60872085068183;10.65310268259;7.51062823758788]
Robot Arm 2 end: [10.4558676186231;6.77409251600167;0.485338341126679]
Robot Arm 1 togoal: 26.4742383240326

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003144e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999447e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-4.92233813900348;-6.20648524781635;-9.16501783223153]
Robot Arm 1 end: [-5.62721207042238;-8.63762190469311;0.810279986455683]
Robot Arm 1 togoal: 24.3326083742353
Mobile Platform 2 Pose: [-3.52053155874385;10.6000120280748;7.44462617047865]
Robot Arm 2 end: [10.8205236980334;6.72132424828655;0.607748772176019]
Robot Arm 1 togoal: 26.7194857637985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004267e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999453e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-5.22287165360109;-5.98395989391737;-9.06635052105099]
Robot Arm 1 end: [-5.47484831071499;-8.41649723620399;0.700101827342411]
Robot Arm 1 togoal: 24.0689047216657
Mobile Platform 2 Pose: [-3.60419695778099;10.6331155855049;7.42205726769781]
Robot Arm 2 end: [10.654380645809;6.65955300910844;0.48678845225619]
Robot Arm 1 togoal: 26.5558607439252

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969726e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-5.43627169794772;-5.85065802474049;-9.03103415038808]
Robot Arm 1 end: [-5.2747591446143;-8.52536000107339;0.892558730092026]
Robot Arm 1 togoal: 24.0187878238073
Mobile Platform 2 Pose: [-3.71562008523373;10.6697350644977;7.34708396824489]
Robot Arm 2 end: [10.2341592702659;6.6794520930925;0.649254480723548]
Robot Arm 1 togoal: 26.2370970907727

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007919e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.840225e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-5.71535807650644;-5.65867412089788;-8.97315415673363]
Robot Arm 1 end: [-4.94082257213059;-8.56501522821927;0.681182959452085]
Robot Arm 1 togoal: 23.8451936921782
Mobile Platform 2 Pose: [-3.62588666262981;10.6207466833408;7.32670513570561]
Robot Arm 2 end: [10.3802337216287;6.59261123696681;0.738596420130231]
Robot Arm 1 togoal: 26.2922199437613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.957979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-5.81180817592017;-5.58992326596527;-8.9522911641209]
Robot Arm 1 end: [-4.975873811966;-8.44173198593454;0.821592583231271]
Robot Arm 1 togoal: 23.7668560354036
Mobile Platform 2 Pose: [-3.55414726834146;10.5569818023467;7.25862507612972]
Robot Arm 2 end: [10.4933347780778;6.52760559614512;0.313828746568735]
Robot Arm 1 togoal: 26.3551850591722

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993510e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999237e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-6.19049267485397;-5.34526551064784;-8.9008841165333]
Robot Arm 1 end: [-4.62025309978009;-8.51684871258922;0.712307782308409]
Robot Arm 1 togoal: 23.6067674629438
Mobile Platform 2 Pose: [-3.55664491976809;10.5552622637483;7.24450855986739]
Robot Arm 2 end: [10.2901347767175;6.50488246648635;0.486281229220526]
Robot Arm 1 togoal: 26.1757780672226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.792602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.235176e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-6.43320824869553;-5.17794897726171;-8.86647124711467]
Robot Arm 1 end: [-4.57333430626408;-8.3858940890939;0.87413842445509]
Robot Arm 1 togoal: 23.470015001176
Mobile Platform 2 Pose: [-3.47190621630091;10.5016861716328;7.19744860824484]
Robot Arm 2 end: [10.3866214746291;6.4600599784223;0.856721453944269]
Robot Arm 1 togoal: 26.2104783556084

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.983769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-6.61349282066965;-5.0643617307198;-8.85126965835639]
Robot Arm 1 end: [-4.32672788606354;-8.49795897102461;0.663975238896559]
Robot Arm 1 togoal: 23.4129047633173
Mobile Platform 2 Pose: [-3.46616151667218;10.4910923323667;7.16552243797048]
Robot Arm 2 end: [10.560409185619;6.38266746679045;0.522207088378166]
Robot Arm 1 togoal: 26.3081699570272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.677596e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998954e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-6.80659085538336;-4.92595123656959;-8.81789895999903]
Robot Arm 1 end: [-4.31390408837508;-8.35189556624549;0.869482831494322]
Robot Arm 1 togoal: 23.283135436329
Mobile Platform 2 Pose: [-3.52463215556326;10.5160655512266;7.12907893962266]
Robot Arm 2 end: [10.3813594439749;6.33946439074925;0.70123927521109]
Robot Arm 1 togoal: 26.1352236838901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015238e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.486438e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-6.99048244221063;-4.81051946125586;-8.80177828351056]
Robot Arm 1 end: [-4.06585509932751;-8.46381157166529;0.663701924206155]
Robot Arm 1 togoal: 23.2270440956226
Mobile Platform 2 Pose: [-3.4536662323987;10.4714060293249;7.0246878325671]
Robot Arm 2 end: [10.6198809780299;6.2878277878907;0.856401180630945]
Robot Arm 1 togoal: 26.2852528461647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.606443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.167723e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-7.18187099629402;-4.67009672246296;-8.76617708010948]
Robot Arm 1 end: [-4.05061403368322;-8.30957436788602;0.868476312586728]
Robot Arm 1 togoal: 23.0886564229996
Mobile Platform 2 Pose: [-3.51501502270095;10.4965667095293;7.00189342578768]
Robot Arm 2 end: [10.4701302029507;6.20353070229472;0.738128340983362]
Robot Arm 1 togoal: 26.1188346941523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.306671e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-7.34692318798009;-4.56590669857591;-8.75201700928899]
Robot Arm 1 end: [-3.81719724847915;-8.42294169504845;0.664843123817391]
Robot Arm 1 togoal: 23.0445812524153
Mobile Platform 2 Pose: [-3.59602999203555;10.5197418099674;6.90504305739226]
Robot Arm 2 end: [10.1009792033097;6.25071117562632;0.914463665705686]
Robot Arm 1 togoal: 25.8554223514205

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.666563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.779699e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-7.57179177586766;-4.40368045549912;-8.71513069217772]
Robot Arm 1 end: [-3.78036246954536;-8.26401338505422;0.869236830506768]
Robot Arm 1 togoal: 22.8888044674815
Mobile Platform 2 Pose: [-3.51438395772721;10.4737076078776;6.88317278171869]
Robot Arm 2 end: [10.3733651365914;6.17835541671234;1.01296759331343]
Robot Arm 1 togoal: 26.0205935348252

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.899202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','3.791942e-14','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-7.71236275249174;-4.31521641877182;-8.70448164074235]
Robot Arm 1 end: [-3.56654310331072;-8.37662032144366;0.676506331771876]
Robot Arm 1 togoal: 22.8574809584994
Mobile Platform 2 Pose: [-3.14640499032715;10.2737858039468;6.82922711876703]
Robot Arm 2 end: [10.0555250376035;6.06768306646527;0.877816533686258]
Robot Arm 1 togoal: 25.7061900839011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.813685e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-7.97978966142822;-4.12655804650231;-8.66751592179906]
Robot Arm 1 end: [-3.50741496957987;-8.21843875194555;0.880309354485326]
Robot Arm 1 togoal: 22.6885930419246
Mobile Platform 2 Pose: [-3.06591879417675;10.2034804460481;6.74161129874061]
Robot Arm 2 end: [9.8293821172669;6.12841790087229;1.04933098389042]
Robot Arm 1 togoal: 25.5646774045463

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.871177e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-8.09459532570406;-4.05470310496825;-8.6600316009325]
Robot Arm 1 end: [-3.31207832146931;-8.32966434162701;0.691473594177906]
Robot Arm 1 togoal: 22.6688362774008
Mobile Platform 2 Pose: [-2.90348518297188;10.098792553033;6.68959670823672]
Robot Arm 2 end: [9.88537331511003;5.94892332789061;0.879444427364724]
Robot Arm 1 togoal: 25.499172942529

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.702064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-8.41508524426616;-3.83981369777617;-8.62757642111345]
Robot Arm 1 end: [-3.09202578062457;-8.37603084203075;0.788437980579428]
Robot Arm 1 togoal: 22.5746826258998
Mobile Platform 2 Pose: [-2.82544516651162;9.99997410862908;6.56430258161658]
Robot Arm 2 end: [9.58962756505366;5.86009380239451;0.778493345065971]
Robot Arm 1 togoal: 25.2160147464044

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.057496e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-8.63675445419393;-3.69218655370511;-8.61116244906338]
Robot Arm 1 end: [-2.88038270343182;-8.51050366356147;0.587702836874985]
Robot Arm 1 togoal: 22.5702026588218
Mobile Platform 2 Pose: [-2.71960814999305;9.93673585793989;6.53276027515002]
Robot Arm 2 end: [9.70643856879214;5.80295331969606;0.894922345895406]
Robot Arm 1 togoal: 25.2679446714417

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.278095e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-8.78377020561651;-3.58659674605434;-8.59889367216789]
Robot Arm 1 end: [-2.84186776709792;-8.3410302784369;0.795938643320433]
Robot Arm 1 togoal: 22.4016221152201
Mobile Platform 2 Pose: [-2.59904144377146;9.83963607587081;6.43202984830301]
Robot Arm 2 end: [9.80150829277549;5.6533187853079;0.710179806459368]
Robot Arm 1 togoal: 25.2543768295633

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.050069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-8.91764378178696;-3.50156204653983;-8.59389634070403]
Robot Arm 1 end: [-2.6956060469938;-8.48979060530392;0.612369594761099]
Robot Arm 1 togoal: 22.4471908875426
Mobile Platform 2 Pose: [-2.70429738867762;9.85765538483404;6.35824430193798]
Robot Arm 2 end: [9.617109249869;5.5891291743288;0.604780225800925]
Robot Arm 1 togoal: 25.0737189584999

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.277871e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-9.12013377135837;-3.35775460788371;-8.58055321819354]
Robot Arm 1 end: [-2.62194612976492;-8.32598202164297;0.807012645780128]
Robot Arm 1 togoal: 22.2635333921248
Mobile Platform 2 Pose: [-2.64688406577385;9.82186321951921;6.32320165365119]
Robot Arm 2 end: [9.72265416579519;5.55603992779729;0.756538122559208]
Robot Arm 1 togoal: 25.1308150990142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.305097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-9.22161123740175;-3.29155267771532;-8.57631646203833]
Robot Arm 1 end: [-2.50394131407162;-8.39020261950041;0.619388437638348]
Robot Arm 1 togoal: 22.2567069482754
Mobile Platform 2 Pose: [-2.65165473588478;9.80998465402353;6.26925482922757]
Robot Arm 2 end: [9.90563434142181;5.51481034883576;0.57842159518728]
Robot Arm 1 togoal: 25.2553375099988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.993754e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-9.40858237019733;-3.16002867346715;-8.56331245990705]
Robot Arm 1 end: [-2.40916318359275;-8.28330832272465;0.629807059930535]
Robot Arm 1 togoal: 22.1147217707408
Mobile Platform 2 Pose: [-2.93036855698512;9.90308980796206;6.16532450671187]
Robot Arm 2 end: [9.79864883992094;5.40568474546218;0.471741478282674]
Robot Arm 1 togoal: 25.1082170177109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919147e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-9.60055055372112;-3.03178205608443;-8.54943996301311]
Robot Arm 1 end: [-2.25505102310257;-8.37477032376736;0.524732321796191]
Robot Arm 1 togoal: 22.1090707474711
Mobile Platform 2 Pose: [-2.92305716279174;9.89143002275364;6.12299984680469]
Robot Arm 2 end: [9.93437387987073;5.36236909314563;0.621110783235638]
Robot Arm 1 togoal: 25.1831579977365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969520e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-9.82521084720302;-2.87355718041575;-8.52880620182112]
Robot Arm 1 end: [-2.16236572228473;-8.20685711626766;0.725794464895639]
Robot Arm 1 togoal: 21.9099324019462
Mobile Platform 2 Pose: [-2.99258210609898;9.91469280144258;6.08263761175002]
Robot Arm 2 end: [10.1321421975411;5.37892545773058;0.4408846333587]
Robot Arm 1 togoal: 25.3570729166775

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.992580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-9.93279632059676;-2.80451645591879;-8.52262627868424]
Robot Arm 1 end: [-2.02895637841495;-8.3544899149012;0.530184189183985]
Robot Arm 1 togoal: 21.9673577737739
Mobile Platform 2 Pose: [-3.20733058539139;10.0080540027168;6.0170770230249]
Robot Arm 2 end: [10.0465312606021;5.24875021266554;0.615389497691903]
Robot Arm 1 togoal: 25.2033317046071

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-10.1848824389637;-2.63310644073819;-8.50403649720306]
Robot Arm 1 end: [-1.88577734423138;-8.26345760718523;0.706189670617861]
Robot Arm 1 togoal: 21.8057217885344
Mobile Platform 2 Pose: [-3.27508319788005;10.034828732758;5.96528581658495]
Robot Arm 2 end: [10.2504929299205;5.26169565964773;0.438935540347958]
Robot Arm 1 togoal: 25.3805571610891

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996500e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-10.3360647256051;-2.53576586864699;-8.4956151110777]
Robot Arm 1 end: [-1.74938080874859;-8.39930119638196;0.502552512254111]
Robot Arm 1 togoal: 21.8545068393105
Mobile Platform 2 Pose: [-3.49631683815162;10.1324930709161;5.8969963637694]
Robot Arm 2 end: [10.168319507494;5.12811800739809;0.612475655708581]
Robot Arm 1 togoal: 25.2278909678612

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.915055e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-10.529371017291;-2.40042858260712;-8.48100969140031]
Robot Arm 1 end: [-1.65942367281049;-8.22431787345563;0.710828778478844]
Robot Arm 1 togoal: 21.6500415644642
Mobile Platform 2 Pose: [-3.55894898105868;10.1588805194335;5.84461221272592]
Robot Arm 2 end: [10.3726770437037;5.14107377399767;0.437166919235931]
Robot Arm 1 togoal: 25.4061563229004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994737e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-10.6406491862679;-2.32819065977447;-8.4751783798037]
Robot Arm 1 end: [-1.54493675752312;-8.38441392877056;0.508023214305773]
Robot Arm 1 togoal: 21.7324265144953
Mobile Platform 2 Pose: [-3.78440383975505;10.2598956663977;5.77428387359077]
Robot Arm 2 end: [10.2930120343453;5.0046277677323;0.609831522318534]
Robot Arm 1 togoal: 25.2542158830759

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-10.8556567567687;-2.17882067004002;-8.46047702154206]
Robot Arm 1 end: [-1.41952669522965;-8.28576903583267;0.721063821148243]
Robot Arm 1 togoal: 21.5734753126406
Mobile Platform 2 Pose: [-3.84276904908034;10.2861415104145;5.72094263898491]
Robot Arm 2 end: [10.4979239339953;5.01735541170305;0.435605856948347]
Robot Arm 1 togoal: 25.4335198038147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004976e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-10.9714520015474;-2.10342139614279;-8.45499631598633]
Robot Arm 1 end: [-1.31538179895246;-8.45309770123106;0.514481328896144]
Robot Arm 1 togoal: 21.669512220226
Mobile Platform 2 Pose: [-4.07018498779505;10.3894352240082;5.64932022842286]
Robot Arm 2 end: [10.4197730413713;4.87878471076952;0.607531515688863]
Robot Arm 1 togoal: 25.2820052646615

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967880e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-11.1745995328872;-1.96124602418278;-8.44354604900207]
Robot Arm 1 end: [-1.19359932235556;-8.33998065916892;0.552207464346441]
Robot Arm 1 togoal: 21.5079082047806
Mobile Platform 2 Pose: [-4.12513246711782;10.4157270656119;5.5948149454615]
Robot Arm 2 end: [10.6253196815911;4.8910701304831;0.434247111982255]
Robot Arm 1 togoal: 25.4623018095067

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005210e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-11.3309926245328;-1.8616214068653;-8.43842461211593]
Robot Arm 1 end: [-1.13723611298852;-8.51092015391714;0.357334699467244]
Robot Arm 1 togoal: 21.6343779691679
Mobile Platform 2 Pose: [-4.35266452245523;10.5203276857577;5.52254966167005]
Robot Arm 2 end: [10.547822346085;4.75114852284835;0.605596629337478]
Robot Arm 1 togoal: 25.3110124104198

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.905886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-11.4705324205063;-1.76425524508459;-8.43195762773282]
Robot Arm 1 end: [-1.06257090492484;-8.50374761421613;0.357053720039921]
Robot Arm 1 togoal: 21.5899032359974
Mobile Platform 2 Pose: [-4.40492048559541;10.5467708437923;5.46685913349285]
Robot Arm 2 end: [10.7539962076866;4.76285461364623;0.433067920720586]
Robot Arm 1 togoal: 25.4922307996269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-11.5868919110612;-1.67715321850659;-8.4266105120969]
Robot Arm 1 end: [-1.08299845040768;-8.54020727653887;0.210038849860949]
Robot Arm 1 togoal: 21.6400267940208
Mobile Platform 2 Pose: [-4.63129336617562;10.6519228491075;5.39443782862892]
Robot Arm 2 end: [10.6764409025414;4.62288631551271;0.59243265400999]
Robot Arm 1 togoal: 25.3417886091084

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999537e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-11.6478940260488;-1.63300723855431;-8.4248262619138]
Robot Arm 1 end: [-1.11812369103137;-8.56093061181407;0.140304355694747]
Robot Arm 1 togoal: 21.6801093181712
Mobile Platform 2 Pose: [-4.68143680390402;10.6788633335402;5.33665341012361]
Robot Arm 2 end: [10.8826304242596;4.6353954963381;0.420978880089376]
Robot Arm 1 togoal: 25.5242845391344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-11.681431301808;-1.61113852195879;-8.42451467532324]
Robot Arm 1 end: [-1.13978564588166;-8.61180761281333;0.144807638785562]
Robot Arm 1 togoal: 21.7344913045517
Mobile Platform 2 Pose: [-4.90032334460928;10.7817872783947;5.26554350083406]
Robot Arm 2 end: [10.8040527407132;4.49538544384527;0.591312979641994]
Robot Arm 1 togoal: 25.3729999716203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988701e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-11.7165663326586;-1.59277808536724;-8.42480230197891]
Robot Arm 1 end: [-1.16102508939286;-8.66829013971239;0.169846788085442]
Robot Arm 1 togoal: 21.7921810011896
Mobile Platform 2 Pose: [-4.94749994542765;10.8076279017272;5.21048392276393]
Robot Arm 2 end: [11.0091668061449;4.50900384554656;0.419926883751108]
Robot Arm 1 togoal: 25.5559903835839

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-11.7168847709743;-1.59281753465919;-8.42489915505297]
Robot Arm 1 end: [-1.20806865485612;-8.71121121298156;0.190614427611997]
Robot Arm 1 togoal: 21.8517769025769
Mobile Platform 2 Pose: [-5.16431878807144;10.910571856666;5.13959677974174]
Robot Arm 2 end: [10.9298400188361;4.37100764448442;0.561977765970761]
Robot Arm 1 togoal: 25.4067665706848

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-11.7178991308944;-1.59235854032;-8.42472101530286]
Robot Arm 1 end: [-1.23107413330967;-8.71811258017575;0.181145211894861]
Robot Arm 1 togoal: 21.8700703155074
Mobile Platform 2 Pose: [-5.20969017530776;10.9374398524462;5.08090833238056]
Robot Arm 2 end: [11.1342051100353;4.38734560217464;0.392526244705455]
Robot Arm 1 togoal: 25.5914788968095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-11.7187581680337;-1.59220692775276;-8.42466575398217]
Robot Arm 1 end: [-1.24867932843579;-8.72308156547646;0.171878368320648]
Robot Arm 1 togoal: 21.883937011391
Mobile Platform 2 Pose: [-5.40928736212823;11.0337959768708;5.01382891741569]
Robot Arm 2 end: [11.0520559919945;4.2494275839765;0.561598701334674]
Robot Arm 1 togoal: 25.4392550758261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-11.719372550116;-1.59232204638747;-8.42466407685282]
Robot Arm 1 end: [-1.260647166522;-8.72677689050591;0.16282573258133]
Robot Arm 1 togoal: 21.8938115607211
Mobile Platform 2 Pose: [-5.4506409587236;11.0578739794067;4.96281222138916]
Robot Arm 2 end: [11.2531459658811;4.26931264639863;0.391442912425604]
Robot Arm 1 togoal: 25.6239223395212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-11.7197758027105;-1.59261284071288;-8.42468079647815]
Robot Arm 1 end: [-1.26696691376333;-8.72981389078224;0.154211264639254]
Robot Arm 1 togoal: 21.9001959641069
Mobile Platform 2 Pose: [-5.64820846996566;11.1543779154739;4.89599770536956]
Robot Arm 2 end: [11.1704068554417;4.13064997348405;0.559761660960945]
Robot Arm 1 togoal: 25.4712656584335

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-11.72005671897;-1.59297137498286;-8.42470203960473]
Robot Arm 1 end: [-1.26777654650347;-8.7326797785347;0.14636064418625]
Robot Arm 1 togoal: 21.9035548107062
Mobile Platform 2 Pose: [-5.68810839201658;11.1790514548769;4.8431981058693]
Robot Arm 2 end: [11.3725903961589;4.14962832273814;0.390532416295441]
Robot Arm 1 togoal: 25.6569202920269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-11.720329826742;-1.59328565336142;-8.42472636848013]
Robot Arm 1 end: [-1.26328303198506;-8.73570212206957;0.139638746047734]
Robot Arm 1 togoal: 21.9042515868369
Mobile Platform 2 Pose: [-5.88294253699616;11.2751331859405;4.77697186425579]
Robot Arm 2 end: [11.28877250311;4.01169718529733;0.550757136564179]
Robot Arm 1 togoal: 25.504526341724

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-11.7196966662912;-1.5939581290542;-8.42475852082751]
Robot Arm 1 end: [-1.25422732607194;-8.7390371612822;0.134419961961757]
Robot Arm 1 togoal: 21.9027795186069
Mobile Platform 2 Pose: [-5.92198983944487;11.3006954046189;4.72172122221445]
Robot Arm 2 end: [11.4916452072944;4.03042041040781;0.382600806014477]
Robot Arm 1 togoal: 25.6912331999434

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-11.719448921875;-1.59430217068676;-8.42480510240277]
Robot Arm 1 end: [-1.2402873316691;-8.74268947727625;0.131070296241692]
Robot Arm 1 togoal: 21.8989582565352
Mobile Platform 2 Pose: [-6.11038312395476;11.3945444413204;4.65704489438928]
Robot Arm 2 end: [11.4062684273146;3.892742581536;0.547240318510587]
Robot Arm 1 togoal: 25.5379097096134

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-11.719673998808;-1.59423760918308;-8.4248715171487]
Robot Arm 1 end: [-1.18639554182791;-8.74485820969537;0.144631302455233]
Robot Arm 1 togoal: 21.8723465890469
Mobile Platform 2 Pose: [-6.1477921456791;11.4196878598523;4.60293808455695]
Robot Arm 2 end: [11.6085699474916;3.91216811370432;0.379231363794282]
Robot Arm 1 togoal: 25.7251307560959

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-11.7209087133284;-1.59337992641244;-8.42497049390484]
Robot Arm 1 end: [-1.0740877618358;-8.73028977559912;0.197319774305264]
Robot Arm 1 togoal: 21.7993418289326
Mobile Platform 2 Pose: [-6.32994056546669;11.5113063857665;4.53996734416765]
Robot Arm 2 end: [11.520816659351;3.77697926986819;0.534596291490976]
Robot Arm 1 togoal: 25.5719318739108

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-11.7249445091387;-1.59050765803629;-8.42515198305936]
Robot Arm 1 end: [-0.97119158673351;-8.6900439411064;0.262683583670565]
Robot Arm 1 togoal: 21.7087113287939
Mobile Platform 2 Pose: [-6.3667171484282;11.5372172131466;4.48391203683232]
Robot Arm 2 end: [11.7232185912492;3.79705468627831;0.367765654168205]
Robot Arm 1 togoal: 25.7601803843792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-11.7336829219665;-1.58442753748544;-8.4253979155584]
Robot Arm 1 end: [-0.889636605865125;-8.64842069404885;0.337956696276377]
Robot Arm 1 togoal: 21.6274679805222
Mobile Platform 2 Pose: [-6.54219011389965;11.6262806219981;4.42258545626312]
Robot Arm 2 end: [11.6342695402924;3.66100098709221;0.534128484490574]
Robot Arm 1 togoal: 25.6054385219133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-11.7477874080173;-1.57474622959794;-8.42562169325857]
Robot Arm 1 end: [-0.809582371434025;-8.59403262299653;0.456064920434346]
Robot Arm 1 togoal: 21.5341404972097
Mobile Platform 2 Pose: [-6.57704217036527;11.6510180994068;4.36972467539546]
Robot Arm 2 end: [11.8348966740522;3.68008228121167;0.365441193925227]
Robot Arm 1 togoal: 25.7922988533701

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-11.7708301058729;-1.55919825839293;-8.42575495959996]
Robot Arm 1 end: [-0.753296319474584;-8.53280917977922;0.57601570321181]
Robot Arm 1 togoal: 21.4474099403567
Mobile Platform 2 Pose: [-6.72487953937033;11.7277742369572;4.31709586360533]
Robot Arm 2 end: [11.7334221001455;3.55411002144326;0.532941367211019]
Robot Arm 1 togoal: 25.6326314395989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-11.8089043167699;-1.53371589403667;-8.42554616014474]
Robot Arm 1 end: [-0.713843424716541;-8.45978467402786;0.67794943406159]
Robot Arm 1 togoal: 21.3602627651802
Mobile Platform 2 Pose: [-6.75966933398049;11.7536932124271;4.2610446402727]
Robot Arm 2 end: [11.9362787250787;3.57025086724951;0.36401846257669]
Robot Arm 1 togoal: 25.8203531963133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-11.8743158070265;-1.48997402026148;-8.42417910651228]
Robot Arm 1 end: [-0.69243455218994;-8.4473487118342;0.837943025197351]
Robot Arm 1 togoal: 21.3330588050496
Mobile Platform 2 Pose: [-6.89508806231882;11.8248884484495;4.21245422827286]
Robot Arm 2 end: [11.8288849931571;3.44866141830941;0.531786068038131]
Robot Arm 1 togoal: 25.6582395770246

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-11.9963586973781;-1.40763838907884;-8.42076503139085]
Robot Arm 1 end: [-0.658157249281632;-8.31120682713208;0.947932277237075]
Robot Arm 1 togoal: 21.1949290640779
Mobile Platform 2 Pose: [-6.92974774780722;11.8516307568398;4.15448152222681]
Robot Arm 2 end: [12.0330029934602;3.46258009089041;0.362735730931082]
Robot Arm 1 togoal: 25.8463896846439

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.837103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-12.1621971369282;-1.29964302812718;-8.41485755867849]
Robot Arm 1 end: [-0.54340792125892;-8.16316495722905;0.832618980271304]
Robot Arm 1 togoal: 21.0128027708324
Mobile Platform 2 Pose: [-7.05441539684163;11.9178793544381;4.10945427209061]
Robot Arm 2 end: [11.9203068146094;3.3449350572059;0.530776289325189]
Robot Arm 1 togoal: 25.6820498699936

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.648203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-12.3524353426422;-1.17579505871323;-8.40191574218515]
Robot Arm 1 end: [-0.475297695557061;-8.14225164480498;0.974403414968981]
Robot Arm 1 togoal: 20.9564291896482
Mobile Platform 2 Pose: [-7.08902933773039;11.9453859365838;4.04973352719296]
Robot Arm 2 end: [12.1256050835354;3.3568595142556;0.361649838619348]
Robot Arm 1 togoal: 25.8706820395756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.508879e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-12.5655819594386;-1.03425420269111;-8.38217728534355]
Robot Arm 1 end: [-0.370966460712585;-7.9718720131357;0.84925208469147]
Robot Arm 1 togoal: 20.7604521686662
Mobile Platform 2 Pose: [-7.20509944036482;12.0076030348426;4.00760093758877]
Robot Arm 2 end: [12.0085240695861;3.24245474135343;0.529902608261668]
Robot Arm 1 togoal: 25.7044745781402

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.145840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-12.8236038708838;-0.870577284841572;-8.35889200709769]
Robot Arm 1 end: [-0.204000660357216;-7.93640892267388;0.823052372088053]
Robot Arm 1 togoal: 20.6476033817306
Mobile Platform 2 Pose: [-7.23970565651525;12.0357836963008;3.94637313509888]
Robot Arm 2 end: [12.2148831416532;3.25262309281655;0.360717129699241]
Robot Arm 1 togoal: 25.8936152009849
Step 151:
Mobile Platform 1 Pose: [-13.0724587266432;-0.705803660990077;-8.32227497223673]
Robot Arm 1 end: [-0.201569870350476;-7.80465599383337;0.958281888902579]
Robot Arm 1 togoal: 20.5278945474432
Mobile Platform 2 Pose: [-7.34871780085855;12.0946360042921;3.90664811593788]
Robot Arm 2 end: [12.0940939948093;3.14095317054336;0.529142033830667]
Robot Arm 1 togoal: 25.7258123663167
Step 152:
Mobile Platform 1 Pose: [-13.2423912315682;-0.601644475068116;-8.30578522033728]
Robot Arm 1 end: [-0.123384288121522;-7.68877127123114;0.811497980234733]
Robot Arm 1 togoal: 20.3931099062151
Mobile Platform 2 Pose: [-7.38334160897926;12.1234109995706;3.84411762717468]
Robot Arm 2 end: [12.3014079189626;3.14957540757751;0.35991573561513]
Robot Arm 1 togoal: 25.9154827320762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.174955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-13.4726576625919;-0.461831125548212;-8.2802774650219]
Robot Arm 1 end: [0.16668751227477;-7.7583512362823;0.672729741000852]
Robot Arm 1 togoal: 20.3168067260759
Mobile Platform 2 Pose: [-7.48651672203207;12.1794411616355;3.80640588350447]
Robot Arm 2 end: [12.1774670847403;3.04023089532816;0.528478562465581]
Robot Arm 1 togoal: 25.7463155985895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.846152e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-13.5057026164322;-0.429494565045998;-8.25026367968248]
Robot Arm 1 end: [0.0202604848046833;-7.6209063605381;0.839808728840412]
Robot Arm 1 togoal: 20.2621707451573
Mobile Platform 2 Pose: [-7.52116918252771;12.2087335542773;3.74276283069617]
Robot Arm 2 end: [12.3856343601831;3.04749756896948;0.359224034349362]
Robot Arm 1 togoal: 25.9365291030708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.417129e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-13.6788605821205;-0.316373936417011;-8.19404647279782]
Robot Arm 1 end: [-0.314059075701748;-7.42098418038472;0.694560146130534]
Robot Arm 1 togoal: 20.2621200090796
Mobile Platform 2 Pose: [-7.61945621488851;12.2623711550711;3.70675303265242]
Robot Arm 2 end: [12.2589870814406;2.94015719573877;0.52789823901635]
Robot Arm 1 togoal: 25.7661876351516

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988246e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-13.8123862836672;-0.241874091488607;-8.1787023834552]
Robot Arm 1 end: [-0.0586519926060428;-7.41138254001768;0.532051423738578]
Robot Arm 1 togoal: 20.1323307431027
Mobile Platform 2 Pose: [-7.65413892257624;12.2921092427039;3.64216916031976]
Robot Arm 2 end: [12.4679135700672;2.94623896226283;0.358624875779531]
Robot Arm 1 togoal: 25.9569520435168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.270832e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-13.9308703091802;-0.173949792614119;-8.12106502860497]
Robot Arm 1 end: [-0.103156699210805;-7.34740853250944;0.687121231934115]
Robot Arm 1 togoal: 20.0923296778253
Mobile Platform 2 Pose: [-7.74828795410005;12.3436993246416;3.60761185549291]
Robot Arm 2 end: [12.3389211659457;2.84064684449318;0.527389568489128]
Robot Arm 1 togoal: 25.7855942714714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996335e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-14.1945728832375;-0.0326946738188792;-8.01604293810546]
Robot Arm 1 end: [-0.0622724860977361;-7.25799346830959;0.534633257409957]
Robot Arm 1 togoal: 20.0015145798408
Mobile Platform 2 Pose: [-7.78299557652216;12.3738165208937;3.54224249060983]
Robot Arm 2 end: [12.5485199063807;2.84569623641623;0.358103902912795]
Robot Arm 1 togoal: 25.9769126167708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-14.4305624499265;0.0968021934943046;-7.98302203030498]
Robot Arm 1 end: [0.210926060112765;-7.24735442168627;0.384608239243535]
Robot Arm 1 togoal: 19.8642540605419
Mobile Platform 2 Pose: [-7.87360406203861;12.4236376214792;3.50893666556103]
Robot Arm 2 end: [12.4174770060914;2.74164764960512;0.526942839771036]
Robot Arm 1 togoal: 25.804670309522

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999442e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-14.3978339298703;0.0758805367910211;-7.94850746156323]
Robot Arm 1 end: [-0.0158741531773037;-7.18661506935654;0.635589177242533]
Robot Arm 1 togoal: 19.9118413241863
Mobile Platform 2 Pose: [-7.90832664434152;12.4540727495114;3.44292124405283]
Robot Arm 2 end: [12.6276684295885;2.74580041093391;0.357649333144572]
Robot Arm 1 togoal: 25.9965419132782

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-14.4407467174395;0.083992501331468;-7.87893328551632]
Robot Arm 1 end: [-0.109294081609165;-7.15509308242512;0.486225638229733]
Robot Arm 1 togoal: 19.9390522210788
Mobile Platform 2 Pose: [-7.99587585560796;12.50235186397;3.41070332636415]
Robot Arm 2 end: [12.4948174963724;2.64312996589804;0.526549860331964]
Robot Arm 1 togoal: 25.8235257199705

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.905939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-14.7130364788577;0.221950436870915;-7.82014699773388]
Robot Arm 1 end: [0.210610052510563;-7.07748352233331;0.388321830334095]
Robot Arm 1 togoal: 19.7168851200084
Mobile Platform 2 Pose: [-8.0306004687371;12.5330490803923;3.34416656001507]
Robot Arm 2 end: [12.7055292060862;2.64650657769318;0.357251395429741]
Robot Arm 1 togoal: 26.0159469549488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973234e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-14.6054698009156;0.157430972995604;-7.79338669686672]
Robot Arm 1 end: [-0.0622927176655009;-7.06538469209384;0.551116314141342]
Robot Arm 1 togoal: 19.8347615006894
Mobile Platform 2 Pose: [-8.11548182988431;12.5799727039883;3.31290232537074]
Robot Arm 2 end: [12.5710713270077;2.54507954160471;0.526203671733734]
Robot Arm 1 togoal: 25.8422503139488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.977211e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-14.5764902236846;0.104045950561604;-7.68181346610419]
Robot Arm 1 end: [-0.179186999331633;-7.00064963253409;0.385335671856385]
Robot Arm 1 togoal: 19.8476111751959
Mobile Platform 2 Pose: [-8.15019382646677;12.6108813480208;3.24595531052739]
Robot Arm 2 end: [12.7822377948208;2.54778667876821;0.356901974153536]
Robot Arm 1 togoal: 26.0352152610539

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-14.6146464338298;0.115093906650217;-7.65201964433846]
Robot Arm 1 end: [-0.0549566841694022;-6.89191709474863;0.52804761370969]
Robot Arm 1 togoal: 19.683113866912
Mobile Platform 2 Pose: [-8.23272872461735;12.6566039797489;3.21553368016258]
Robot Arm 2 end: [12.646341076789;2.44749231985536;0.525898332019481]
Robot Arm 1 togoal: 25.8609175504029

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-14.3865244021134;-0.0430407976892302;-7.55758726919589]
Robot Arm 1 end: [-0.405180974109962;-6.88377173878734;0.695360239139832]
Robot Arm 1 togoal: 19.8497158731849
Mobile Platform 2 Pose: [-8.26741265637282;12.6876783180323;3.14827508873206]
Robot Arm 2 end: [12.857903211793;2.44962425569019;0.356594287412353]
Robot Arm 1 togoal: 26.0544185122423

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-14.3954247402644;-0.0518356401139199;-7.52684273684585]
Robot Arm 1 end: [-0.362085461452543;-6.8398205642348;0.413925689761487]
Robot Arm 1 togoal: 19.8034841700763
Mobile Platform 2 Pose: [-8.34786725655752;12.7323289284606;3.11860342342018]
Robot Arm 2 end: [12.7207092142224;2.35037081137509;0.525628749113669]
Robot Arm 1 togoal: 25.8795875218831

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997208e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-14.3285926214036;-0.106866908447373;-7.48811813365836]
Robot Arm 1 end: [-0.293878449996212;-6.73554681386181;0.55431805113971]
Robot Arm 1 togoal: 19.6717293264346
Mobile Platform 2 Pose: [-8.38250753236709;12.7635278478683;3.05112066202503]
Robot Arm 2 end: [12.932613905923;2.35201073118325;0.356322638959733]
Robot Arm 1 togoal: 26.0736154573301

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-14.1480662012129;-0.219821181282836;-7.45661605015998]
Robot Arm 1 end: [-0.584777304844778;-6.80730225529724;0.439195382151287]
Robot Arm 1 togoal: 19.8920391344195
Mobile Platform 2 Pose: [-8.46110402808027;12.8072148670881;3.0221211425014]
Robot Arm 2 end: [12.7942426473168;2.25372155108872;0.52539053712703]
Robot Arm 1 togoal: 25.8983093641059

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.874296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-14.1126431483686;-0.266945296568753;-7.40604245423089]
Robot Arm 1 end: [-0.440262114082307;-6.72926228640775;0.26765037198651]
Robot Arm 1 togoal: 19.7595084692967
Mobile Platform 2 Pose: [-8.49568538227997;12.8385015424038;2.95449146712681]
Robot Arm 2 end: [13.0064422803853;2.25494276296858;0.356082220938751]
Robot Arm 1 togoal: 26.0928542316917

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.911354e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-14.0505500820753;-0.307082994078836;-7.39610183265147]
Robot Arm 1 end: [-0.636234950381258;-6.74598244282488;0.378257473164126]
Robot Arm 1 togoal: 19.8711671116155
Mobile Platform 2 Pose: [-8.57261061309497;12.881316731159;2.92609829000165]
Robot Arm 2 end: [12.8669961509855;2.15755333712147;0.525179904000915]
Robot Arm 1 togoal: 25.9171231771146

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.821097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-13.8740892026625;-0.432419072599656;-7.35279027481333]
Robot Arm 1 end: [-0.743368710915947;-6.77865647649893;0.226483346527587]
Robot Arm 1 togoal: 19.9654465960861
Mobile Platform 2 Pose: [-8.60711842379805;12.9126582825006;2.85838984932228]
Robot Arm 2 end: [13.0794481215312;2.15842037178582;0.355868955058129]
Robot Arm 1 togoal: 26.1121742082768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.919162e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.589469e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-13.7889532712988;-0.491950106197705;-7.33484653889819]
Robot Arm 1 end: [-0.716119232121277;-6.66518831133314;0.36051236054033]
Robot Arm 1 togoal: 19.8471772403678
Mobile Platform 2 Pose: [-8.68262258282422;12.9547257834624;2.83054703810713]
Robot Arm 2 end: [12.9388714640933;2.06166435774282;0.525055638179139]
Robot Arm 1 togoal: 25.935833826906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009139e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.373310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-13.6250118377296;-0.600527448321806;-7.30059994363645]
Robot Arm 1 end: [-0.927348899177349;-6.73601901926084;0.23041580585923]
Robot Arm 1 togoal: 20.0291511088108
Mobile Platform 2 Pose: [-8.71781119603862;12.9865347648136;2.76250366790449]
Robot Arm 2 end: [13.145231830456;2.05806996456151;0.359852334701104]
Robot Arm 1 togoal: 26.1236879413387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001031e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.834255e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-13.5824168346606;-0.638832792664525;-7.27544346898125]
Robot Arm 1 end: [-0.809740838298371;-6.64119502109488;0.262306340579217]
Robot Arm 1 togoal: 19.8837667067775
Mobile Platform 2 Pose: [-8.79487729333859;13.0291311194638;2.73374440356219]
Robot Arm 2 end: [13.0040761574989;1.96378828046902;0.529763267775624]
Robot Arm 1 togoal: 25.9480692950209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.846039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.116491e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-13.4020545675073;-0.757191544539033;-7.23445952247522]
Robot Arm 1 end: [-0.915554902246483;-6.5896451066115;0.27139256407866]
Robot Arm 1 togoal: 19.8979190452466
Mobile Platform 2 Pose: [-8.82216101861862;13.056437608076;2.66404839211068]
Robot Arm 2 end: [13.21318268074;1.95903773566236;0.360082795484736]
Robot Arm 1 togoal: 26.1384422353006

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013147e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.924407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-13.1638718358383;-0.924828074030531;-7.13497408668927]
Robot Arm 1 end: [-0.950617042884471;-6.51473711673006;0.185923966959705]
Robot Arm 1 togoal: 19.8603872990587
Mobile Platform 2 Pose: [-8.89634199233884;13.0975113455043;2.63529852991229]
Robot Arm 2 end: [13.0705312117996;1.86542708807226;0.530360893146872]
Robot Arm 1 togoal: 25.9618785186474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.620359e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.694790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-13.0586134766679;-0.991958639666425;-7.10421761899517]
Robot Arm 1 end: [-0.936047505036946;-6.41652953768449;0.318938351643517]
Robot Arm 1 togoal: 19.7621995056355
Mobile Platform 2 Pose: [-8.92264184387985;13.1239337511949;2.5666503414379]
Robot Arm 2 end: [13.2786665873666;1.86180013724993;0.360869017072994]
Robot Arm 1 togoal: 26.152333059966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.701338e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943996e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-12.8222362741495;-1.13692803760131;-7.01020650808365]
Robot Arm 1 end: [-1.29021802814084;-6.38926471763433;0.220104382421326]
Robot Arm 1 togoal: 19.9441557985161
Mobile Platform 2 Pose: [-8.99981907309379;13.166469493256;2.53659489094428]
Robot Arm 2 end: [13.1375958147623;1.76726548458284;0.531149865824001]
Robot Arm 1 togoal: 25.9768489305399

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.054895e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968904e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-12.8594750412021;-1.12760792193522;-6.9680540172398]
Robot Arm 1 end: [-1.11288335532561;-6.27320055917765;0.313683818746742]
Robot Arm 1 togoal: 19.7426149552132
Mobile Platform 2 Pose: [-9.02523023743595;13.1917969025519;2.46983327329995]
Robot Arm 2 end: [13.3445122232747;1.76485226070672;0.361477711056755]
Robot Arm 1 togoal: 26.1672025940381

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.477976e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-12.6069788510983;-1.28601390552114;-6.8659870086969]
Robot Arm 1 end: [-1.46500538203662;-6.13944944216583;0.481262042691794]
Robot Arm 1 togoal: 19.824460439725
Mobile Platform 2 Pose: [-9.10425729714238;13.2351823106486;2.43891824102801]
Robot Arm 2 end: [13.2043760160088;1.66998085543244;0.53189676742322]
Robot Arm 1 togoal: 25.9924959298482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.371534e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.960279e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-12.6666343987468;-1.26214315110468;-6.83151159125319]
Robot Arm 1 end: [-1.24454029377077;-6.12320131334205;0.305178847039231]
Robot Arm 1 togoal: 19.6945557981077
Mobile Platform 2 Pose: [-9.12895620817363;13.2595894682098;2.37369419469006]
Robot Arm 2 end: [13.4102827184122;1.66871626168511;0.362194488600613]
Robot Arm 1 togoal: 26.1828781146182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.104811e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.963193e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-12.5396460797701;-1.34126529106315;-6.78329296085901]
Robot Arm 1 end: [-1.36907791660579;-6.03057221184633;0.507378455686257]
Robot Arm 1 togoal: 19.6789889666154
Mobile Platform 2 Pose: [-9.21077587559238;13.3042840525932;2.34162888751146]
Robot Arm 2 end: [13.2715524559687;1.57316811656643;0.532664328735064]
Robot Arm 1 togoal: 26.0092326177515

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.907632e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957188e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-12.4249615579499;-1.41761566338079;-6.71625521691176]
Robot Arm 1 end: [-1.4959505485363;-6.10026835008251;0.162838019220971]
Robot Arm 1 togoal: 19.8297787159891
Mobile Platform 2 Pose: [-9.23495183101472;13.3278344749206;2.27802408893914]
Robot Arm 2 end: [13.4763643662824;1.57290969953203;0.362981231018417]
Robot Arm 1 togoal: 26.1994833964582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.968258e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-12.4275068872605;-1.41975427272554;-6.69745798590076]
Robot Arm 1 end: [-1.38305079000445;-6.01144356514621;0.292122627162319]
Robot Arm 1 togoal: 19.6837575154023
Mobile Platform 2 Pose: [-9.31959376275018;13.3738263235936;2.24483936504133]
Robot Arm 2 end: [13.338986194521;1.4768024330097;0.533505691709654]
Robot Arm 1 togoal: 26.0269171375191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956459e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-12.2527523399629;-1.52818057635706;-6.61637301722328]
Robot Arm 1 end: [-1.68858357464862;-5.89219232096758;0.454102023269584]
Robot Arm 1 togoal: 19.7565946974907
Mobile Platform 2 Pose: [-9.34341657935675;13.3966385220361;2.18265332642195]
Robot Arm 2 end: [13.5421357903712;1.47689852674647;0.36436615978189]
Robot Arm 1 togoal: 26.2162003197613

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-12.3320014864434;-1.49614709498608;-6.56469877483612]
Robot Arm 1 end: [-1.46032963391328;-5.86914664155931;0.283115953930155]
Robot Arm 1 togoal: 19.6138045101092
Mobile Platform 2 Pose: [-9.43098647545053;13.4439239094115;2.14810383398464]
Robot Arm 2 end: [13.4058311104618;1.38024788160064;0.535030500376304]
Robot Arm 1 togoal: 26.0444832483258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954446e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-12.2155376913379;-1.56649630998422;-6.53032671869693]
Robot Arm 1 end: [-1.72950592410686;-5.80704475889629;0.45310949746974]
Robot Arm 1 togoal: 19.7125433001123
Mobile Platform 2 Pose: [-9.45444056092328;13.4659901828216;2.0867835451502]
Robot Arm 2 end: [13.6094396244842;1.38102272229131;0.365116231746083]
Robot Arm 1 togoal: 26.2348886956509

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974365e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-12.1605784492044;-1.62016015445126;-6.44788145385274]
Robot Arm 1 end: [-1.58622256670612;-5.75203483155517;0.283452377074783]
Robot Arm 1 togoal: 19.5933325092692
Mobile Platform 2 Pose: [-9.54503626336938;13.5146259757873;2.05094305241714]
Robot Arm 2 end: [13.474347085042;1.2834058156512;0.535936820096528]
Robot Arm 1 togoal: 26.0639706748242

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.954979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-12.0461012821106;-1.68882835480571;-6.41906125625723]
Robot Arm 1 end: [-1.85454581947798;-5.69655638060053;0.452621254076317]
Robot Arm 1 togoal: 19.6990754082575
Mobile Platform 2 Pose: [-9.56883264977343;13.536291718034;1.99085057132841]
Robot Arm 2 end: [13.6766966769896;1.28490551928262;0.366789016495186]
Robot Arm 1 togoal: 26.2538999218735

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.975746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-11.9810254569788;-1.74806073576824;-6.33777939848894]
Robot Arm 1 end: [-1.71658914604817;-5.64177427569766;0.283472381354214]
Robot Arm 1 togoal: 19.5825186918354
Mobile Platform 2 Pose: [-9.66671625949997;13.5882743075367;1.95252225740646]
Robot Arm 2 end: [13.5448899036375;1.18555589738451;0.537528744944093]
Robot Arm 1 togoal: 26.0853544545104

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.957781e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-11.8667152092481;-1.8166959159521;-6.30837443761512]
Robot Arm 1 end: [-1.98506094314425;-5.58662926640114;0.452180814831192]
Robot Arm 1 togoal: 19.6907864786986
Mobile Platform 2 Pose: [-9.6909271721519;13.6094091944837;1.89388437889876]
Robot Arm 2 end: [13.7476666024625;1.1878389133003;0.367509302422961]
Robot Arm 1 togoal: 26.2764143178072

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','9.555781e-15','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-11.414020142954;-2.0551241957797;-6.23319494986443]
Robot Arm 1 end: [-2.03074170320089;-5.53683134788119;0.283554148995708]
Robot Arm 1 togoal: 19.6891631740866
Mobile Platform 2 Pose: [-9.79335466141144;13.6634267386626;1.85430569190961]
Robot Arm 2 end: [13.6179598710645;1.08783548709065;0.538246797764131]
Robot Arm 1 togoal: 26.1096520106359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.235701e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.962075e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-11.3015104180375;-2.12266587129786;-6.20441136308351]
Robot Arm 1 end: [-2.29942657920462;-5.48231240029404;0.461648763053879]
Robot Arm 1 togoal: 19.8015180347559
Mobile Platform 2 Pose: [-9.81858791280199;13.6845542228803;1.79693681333414]
Robot Arm 2 end: [13.8203543061324;1.09060536822646;0.368459286166871]
Robot Arm 1 togoal: 26.3009345233293

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.064885e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.979792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-11.2425609034979;-2.17601975512243;-6.13223071456351]
Robot Arm 1 end: [-2.15632549819635;-5.4315075334985;0.291771506384446]
Robot Arm 1 togoal: 19.6829539720042
Mobile Platform 2 Pose: [-9.92604856677818;13.7408436609465;1.75601997954504]
Robot Arm 2 end: [13.6928325819891;0.989913923381032;0.539066156877603]
Robot Arm 1 togoal: 26.1360425059679

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.732199e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-11.1224614174723;-2.24843912076476;-6.09870257298268]
Robot Arm 1 end: [-2.42970778857235;-5.37804907829218;0.460652365203557]
Robot Arm 1 togoal: 19.8016758449955
Mobile Platform 2 Pose: [-9.95315926830563;13.762514300425;1.69950679330277]
Robot Arm 2 end: [13.8855852964746;0.989090021102681;0.377680480974979]
Robot Arm 1 togoal: 26.3170755208409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.969304e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007305e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-11.0741971226687;-2.29344666852962;-6.03515855203362]
Robot Arm 1 end: [-2.27568668403087;-5.39986969573684;0.182970309325385]
Robot Arm 1 togoal: 19.7392703427278
Mobile Platform 2 Pose: [-10.0874229839457;13.8303454062011;1.6495915613826]
Robot Arm 2 end: [13.6355726069296;0.926430784795938;0.453174620789101]
Robot Arm 1 togoal: 26.0608409916654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003385e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.759045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-11.0051278925805;-2.33509929077186;-6.01778189375277]
Robot Arm 1 end: [-2.5128903938261;-5.33418727391782;0.341391999514142]
Robot Arm 1 togoal: 19.8267798796047
Mobile Platform 2 Pose: [-10.0600540611578;13.8195587929084;1.62234013774891]
Robot Arm 2 end: [13.8075222679136;0.88672774034438;0.441070635212103]
Robot Arm 1 togoal: 26.2008626740135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.827147e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005979e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-10.8254415109048;-2.46333902556419;-5.92761921229476]
Robot Arm 1 end: [-2.42215021512822;-5.20642060496323;0.332406215271994]
Robot Arm 1 togoal: 19.6712429390558
Mobile Platform 2 Pose: [-10.2318087819187;13.8936390308455;1.51299172171427]
Robot Arm 2 end: [13.7513582948077;0.825189590808017;0.288556231624558]
Robot Arm 1 togoal: 26.1310288386714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984726e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.902325e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-10.7262637472737;-2.52181303915505;-5.90759119166184]
Robot Arm 1 end: [-2.68215648248865;-5.163093606712;0.504135486933546]
Robot Arm 1 togoal: 19.793698851154
Mobile Platform 2 Pose: [-10.2351756149104;13.8916502831851;1.48667061080918]
Robot Arm 2 end: [13.5640621581327;0.80855423971173;0.463890010381089]
Robot Arm 1 togoal: 25.9462396442198

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000050e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.600551e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-10.6283350286282;-2.59357748147492;-5.84590686689991]
Robot Arm 1 end: [-2.5614098771449;-5.11877079592649;0.326338039252945]
Robot Arm 1 togoal: 19.692472310044
Mobile Platform 2 Pose: [-10.150673860747;13.8426950722798;1.44139387360814]
Robot Arm 2 end: [13.7393297153175;0.691169720672909;0.576780480253233]
Robot Arm 1 togoal: 26.0528009210192

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967687e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-10.5414370928066;-2.6457109407599;-5.82258290922856]
Robot Arm 1 end: [-2.81341914986774;-5.07831279632646;0.503027053688082]
Robot Arm 1 togoal: 19.8135180120412
Mobile Platform 2 Pose: [-10.2242809255501;13.8594402978094;1.39384626001601]
Robot Arm 2 end: [13.6097918056982;0.68278388220044;0.429794840378932]
Robot Arm 1 togoal: 25.937133019719

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999738e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.889260e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-10.4744078649823;-2.69861157655593;-5.76591856338638]
Robot Arm 1 end: [-2.67410397983596;-5.0375658505651;0.323920052530479]
Robot Arm 1 togoal: 19.7026469998934
Mobile Platform 2 Pose: [-10.223861805426;13.8345749228955;1.34298940545445]
Robot Arm 2 end: [13.3946020602144;0.682074751720611;0.606238534476372]
Robot Arm 1 togoal: 25.7342362446594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982961e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.650786e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-10.4046114059444;-2.74106727622411;-5.74612498145177]
Robot Arm 1 end: [-2.91543044864137;-4.99952455675728;0.501420042471862]
Robot Arm 1 togoal: 19.8200308751589
Mobile Platform 2 Pose: [-10.022851166382;13.7045611613437;1.26425184347192]
Robot Arm 2 end: [13.4539371954675;0.534001752932937;0.717079039452024]
Robot Arm 1 togoal: 25.7235038789536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002443e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-10.3257312934212;-2.80343208868471;-5.68217049852716]
Robot Arm 1 end: [-2.78580578882828;-4.95023028688372;0.321030539847071]
Robot Arm 1 togoal: 19.7085296764063
Mobile Platform 2 Pose: [-10.0387856867895;13.6916687846158;1.23033593872077]
Robot Arm 2 end: [13.2187485459034;0.594896359857605;0.534652810478804]
Robot Arm 1 togoal: 25.5408495099966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-10.2694276012164;-2.837759102946;-5.66651693785068]
Robot Arm 1 end: [-3.01102770517043;-4.91464086059555;0.493945308104725]
Robot Arm 1 togoal: 19.8188848143735
Mobile Platform 2 Pose: [-9.85272926982971;13.5625950989954;1.16202067497269]
Robot Arm 2 end: [13.0815021504054;0.585368914608356;0.839251324514373]
Robot Arm 1 togoal: 25.4021762420871

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.941867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002013e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-10.1545082589301;-2.92435961462538;-5.58694027911976]
Robot Arm 1 end: [-2.90572807158814;-4.85888322069006;0.314789886718256]
Robot Arm 1 togoal: 19.7179910351249
Mobile Platform 2 Pose: [-9.78675154106129;13.5194523559366;1.14212007608549]
Robot Arm 2 end: [13.1741401504567;0.432995437630893;0.657684526232926]
Robot Arm 1 togoal: 25.4289889520186

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.927621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-10.1039116915064;-2.95507473999756;-5.57332124944572]
Robot Arm 1 end: [-3.12156268606731;-4.81967214626484;0.483845782156746]
Robot Arm 1 togoal: 19.8210438352465
Mobile Platform 2 Pose: [-9.65445683760992;13.3971760177273;1.02221617985448]
Robot Arm 2 end: [12.8217806668217;0.455485876678498;0.5489116175412]
Robot Arm 1 togoal: 25.1216204402752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.900636e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000553e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-9.98058389141357;-3.04790417925707;-5.48682868067222]
Robot Arm 1 end: [-3.04492451188429;-4.76558421597062;0.320774208196621]
Robot Arm 1 togoal: 19.7391254798937
Mobile Platform 2 Pose: [-9.57202915347727;13.3508766779433;1.00485013263244]
Robot Arm 2 end: [12.9236859126634;0.339706782637577;0.674462094511639]
Robot Arm 1 togoal: 25.1619064976959

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001068e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-9.91995678562195;-3.08481042073955;-5.468956683901]
Robot Arm 1 end: [-3.26236225832968;-4.72666394847794;0.472148140467469]
Robot Arm 1 togoal: 19.8460316926536
Mobile Platform 2 Pose: [-9.33711074415341;13.1992040044012;0.913060435631427]
Robot Arm 2 end: [12.9470839727384;0.17705442415143;0.49964949805099]
Robot Arm 1 togoal: 25.1233806732271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996254e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948536e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-9.84245370628971;-3.14785966769305;-5.39641193569701]
Robot Arm 1 end: [-3.13672665755598;-4.68554663248623;0.292355830186354]
Robot Arm 1 togoal: 19.7420473524477
Mobile Platform 2 Pose: [-9.32454280020742;13.1762634903935;0.871705517686607]
Robot Arm 2 end: [12.6292160915645;0.280744976241542;0.649125476498505]
Robot Arm 1 togoal: 24.8703699886023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.946333e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937198e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-9.79205022221819;-3.1782497541065;-5.38163968432264]
Robot Arm 1 end: [-3.34519114512377;-4.63742058129576;0.459113889377402]
Robot Arm 1 togoal: 19.8362246933625
Mobile Platform 2 Pose: [-9.16444986530737;13.082838564712;0.835432018523016]
Robot Arm 2 end: [12.8418981545876;0.0958927574468976;0.586818294912519]
Robot Arm 1 togoal: 24.9910395408542

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.952769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004069e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-9.70363306080411;-3.24838200597293;-5.30002782954553]
Robot Arm 1 end: [-3.17858790821368;-4.58428382216299;0.323090661867454]
Robot Arm 1 togoal: 19.69293021981
Mobile Platform 2 Pose: [-9.16051491294811;13.0625819401581;0.790847361777583]
Robot Arm 2 end: [12.6359347218347;0.128947668495247;0.472944581698666]
Robot Arm 1 togoal: 24.8209496547193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.942988e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.902617e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-9.6438869043308;-3.2832705461937;-5.28345712028003]
Robot Arm 1 end: [-3.39321849423011;-4.53063559437369;0.45479712299472]
Robot Arm 1 togoal: 19.790261771528
Mobile Platform 2 Pose: [-9.04451533848037;12.99319901982;0.748940657778917]
Robot Arm 2 end: [12.7260315490176;0.0159254674871963;0.603651157257802]
Robot Arm 1 togoal: 24.8522192537504

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008224e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-9.53982577012044;-3.36288215213781;-5.17864662737235]
Robot Arm 1 end: [-3.32245809863289;-4.45134319335599;0.347082636491968]
Robot Arm 1 togoal: 19.6902841995318
Mobile Platform 2 Pose: [-8.95826771357227;12.9291035942725;0.642672052820404]
Robot Arm 2 end: [12.8543187572733;-0.0544140382305567;0.242871544969467]
Robot Arm 1 togoal: 24.9573271471438

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.889282e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.933106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-9.46011766656039;-3.41013048272773;-5.15063940550904]
Robot Arm 1 end: [-3.54654174315475;-4.39385092325157;0.514836317676266]
Robot Arm 1 togoal: 19.7915159337392
Mobile Platform 2 Pose: [-9.00190830871031;12.948847123915;0.63363145683051]
Robot Arm 2 end: [12.9458972975451;-0.0510810640515531;0.245269062942692]
Robot Arm 1 togoal: 25.0424176190583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012610e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.953672e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-9.43365563722084;-3.43735236878937;-5.09234680353372]
Robot Arm 1 end: [-3.38425944693889;-4.35917495230469;0.327684274700394]
Robot Arm 1 togoal: 19.6659174378953
Mobile Platform 2 Pose: [-9.02267397335801;12.9585959459867;0.622072566858839]
Robot Arm 2 end: [12.837518115858;-0.0755599825384149;0.296803799955505]
Robot Arm 1 togoal: 24.9308122548893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.874568e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.936619e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-9.37985113866334;-3.46881780239564;-5.07540209864338]
Robot Arm 1 end: [-3.59228757498264;-4.309922499836;0.498814611993356]
Robot Arm 1 togoal: 19.7628192938101
Mobile Platform 2 Pose: [-9.02745334024723;12.961267916021;0.591014051474217]
Robot Arm 2 end: [12.913148087346;-0.157728831757736;0.455282969425959]
Robot Arm 1 togoal: 24.9603367327269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998759e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004427e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-9.34628963047419;-3.50507862103496;-4.98928627890437]
Robot Arm 1 end: [-3.26383656344295;-4.26886696833532;0.430006527790148]
Robot Arm 1 togoal: 19.5120524727201
Mobile Platform 2 Pose: [-9.06158457155787;12.9880850862609;0.524364811622793]
Robot Arm 2 end: [13.1022547081408;-0.179048186799122;0.297962201476018]
Robot Arm 1 togoal: 25.1328686896988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-9.2857932426864;-3.53780926715028;-4.97295652162914]
Robot Arm 1 end: [-3.47678130224471;-4.19432234699295;0.507089789541109]
Robot Arm 1 togoal: 19.5992457781862
Mobile Platform 2 Pose: [-9.09147439636411;13.0052565767814;0.513711574744946]
Robot Arm 2 end: [13.1573874083446;-0.181902274212272;0.310522542658742]
Robot Arm 1 togoal: 25.1818365138051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995884e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997714e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-9.18333885104513;-3.60221754948194;-4.85037636257069]
Robot Arm 1 end: [-3.71731828394051;-4.07451469598313;0.679628674376437]
Robot Arm 1 togoal: 19.6714181120212
Mobile Platform 2 Pose: [-9.12428672448155;13.0246991657658;0.504141037558222]
Robot Arm 2 end: [13.3103567063478;-0.0957582841486342;0.235654684573442]
Robot Arm 1 togoal: 25.3597879786696

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000049e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-9.22079120454082;-3.58513974137437;-4.82752032336885]
Robot Arm 1 end: [-3.50387401634805;-4.0426962726864;0.489622578854076]
Robot Arm 1 togoal: 19.5093739377042
Mobile Platform 2 Pose: [-9.14263882631362;13.0335268556044;0.504756228708231]
Robot Arm 2 end: [13.4347446093869;0.0724469536598851;0.144359606138346]
Robot Arm 1 togoal: 25.5447955817911

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998087e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-9.15164197216274;-3.62695054422465;-4.75957210313302]
Robot Arm 1 end: [-3.73988742749991;-3.98622950042588;0.679178229039104]
Robot Arm 1 togoal: 19.6241633210952
Mobile Platform 2 Pose: [-9.14757171500789;13.0345112608332;0.506996030833952]
Robot Arm 2 end: [13.5302958567688;0.220452254136718;0.139439686773048]
Robot Arm 1 togoal: 25.691266537196

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-9.18419952734214;-3.61253981469934;-4.73715440961414]
Robot Arm 1 end: [-3.52911554177443;-3.95311104059177;0.489792447306563]
Robot Arm 1 togoal: 19.462519468725
Mobile Platform 2 Pose: [-9.14515516490864;13.0317347531679;0.509111221043591]
Robot Arm 2 end: [13.5742557536438;0.387104020636886;0.14343417901454]
Robot Arm 1 togoal: 25.7979679793756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-9.10642783440678;-3.65918692873138;-4.66723160952786]
Robot Arm 1 end: [-3.77058848497953;-3.8941279941424;0.678578162304135]
Robot Arm 1 togoal: 19.5802342808478
Mobile Platform 2 Pose: [-9.13704819135882;13.0261527534075;0.511073186309568]
Robot Arm 2 end: [13.580675611147;0.527516831865921;0.150481350946021]
Robot Arm 1 togoal: 25.8603015119849

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-9.13812498855658;-3.64525318342365;-4.64468813782503]
Robot Arm 1 end: [-3.55974007854314;-3.86086073964445;0.489993041943802]
Robot Arm 1 togoal: 19.4178522702442
Mobile Platform 2 Pose: [-9.12730358606662;13.0199374681549;0.512954449952069]
Robot Arm 2 end: [13.5676566234278;0.649735234831734;0.15849225218705]
Robot Arm 1 togoal: 25.8980255358332

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-9.05512285082309;-3.69477866168793;-4.57326490200291]
Robot Arm 1 end: [-3.80485097593624;-3.79989115935074;0.677978901721608]
Robot Arm 1 togoal: 19.5376966669533
Mobile Platform 2 Pose: [-9.1174542751061;13.0139164472936;0.514919548002235]
Robot Arm 2 end: [13.5327316363873;0.764843230012241;0.166773995725604]
Robot Arm 1 togoal: 25.9134180707142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991243e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-9.08728056926354;-3.68056856805531;-4.55059543272782]
Robot Arm 1 end: [-3.59305327907771;-3.76696234228002;0.490278717116495]
Robot Arm 1 togoal: 19.374280269073
Mobile Platform 2 Pose: [-9.09735913420609;13.0030524560305;0.517314093320802]
Robot Arm 2 end: [13.4925777642409;0.864933627234866;0.189438317311631]
Robot Arm 1 togoal: 25.9175818557196

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-8.99956764268465;-3.73268079487767;-4.4773377145898]
Robot Arm 1 end: [-3.8414961929753;-3.70371221276939;0.677437947787306]
Robot Arm 1 togoal: 19.4959057806117
Mobile Platform 2 Pose: [-9.08139293249811;12.9951969864622;0.520942687316671]
Robot Arm 2 end: [13.4514129981049;0.962289011190177;0.210623808142312]
Robot Arm 1 togoal: 25.9202167764225

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994845e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-9.03244903171528;-3.71802696463418;-4.45463366598823]
Robot Arm 1 end: [-3.62858372604438;-3.67117404814221;0.490649169814248]
Robot Arm 1 togoal: 19.3313454540977
Mobile Platform 2 Pose: [-9.08476628052683;12.9993356277755;0.526633563495448]
Robot Arm 2 end: [13.4197504176561;1.05366868616182;0.232029534779229]
Robot Arm 1 togoal: 25.9293221259051

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-8.94067400206985;-3.77235840873469;-4.37921797741214]
Robot Arm 1 end: [-3.87997429606635;-3.60535628551442;0.676950061090965]
Robot Arm 1 togoal: 19.4543523771577
Mobile Platform 2 Pose: [-9.08787328695841;13.0018759036789;0.529022056421382]
Robot Arm 2 end: [13.3473937139925;1.11675897827923;0.249284306911508]
Robot Arm 1 togoal: 25.890159988331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-8.97456094084627;-3.75708308692211;-4.35657109063207]
Robot Arm 1 end: [-3.66576943730523;-3.57326545864741;0.491104990478745]
Robot Arm 1 togoal: 19.2885321695336
Mobile Platform 2 Pose: [-9.08618348870936;13.0012431518386;0.530603928280834]
Robot Arm 2 end: [13.259520204616;1.17013301058762;0.283753299214976]
Robot Arm 1 togoal: 25.8323040207752

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-8.87956345936146;-3.81315456081342;-4.27870154450781]
Robot Arm 1 end: [-3.91960802539598;-3.50461578987335;0.676510082777238]
Robot Arm 1 togoal: 19.4124521488929
Mobile Platform 2 Pose: [-9.07934416460393;12.9974125880154;0.531472800362341]
Robot Arm 2 end: [13.1605996913231;1.21003443812865;0.336062913207303]
Robot Arm 1 togoal: 25.7581506724649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-8.91450521832741;-3.79723488778891;-4.25608443988016]
Robot Arm 1 end: [-3.70409408156844;-3.47291431999001;0.491647651388186]
Robot Arm 1 togoal: 19.2452898426567
Mobile Platform 2 Pose: [-9.06716021200182;12.9904146541971;0.531716987237353]
Robot Arm 2 end: [13.0552373935628;1.23380423056313;0.406320204156687]
Robot Arm 1 togoal: 25.6707000257379

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-8.81650775283009;-3.85490170773402;-4.17613935726941]
Robot Arm 1 end: [-3.96023645024675;-3.40184250778328;0.676117724223135]
Robot Arm 1 togoal: 19.3703745552363
Mobile Platform 2 Pose: [-9.05007151126096;12.9806009357418;0.531496942081284]
Robot Arm 2 end: [12.9459275601823;1.2395831774393;0.492930206377829]
Robot Arm 1 togoal: 25.5714831228122

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-8.84894573315512;-3.840680221618;-4.15183456252196]
Robot Arm 1 end: [-3.7458554806157;-3.36876900618299;0.492271092915957]
Robot Arm 1 togoal: 19.202364718933
Mobile Platform 2 Pose: [-9.02903069780085;12.9685540823256;0.531006806023144]
Robot Arm 2 end: [12.832823863924;1.22437974001615;0.592301480350964]
Robot Arm 1 togoal: 25.4595174166649

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-8.73868547597717;-3.90503137359765;-4.07873651346378]
Robot Arm 1 end: [-4.00972061652942;-3.30430557793615;0.675820148385971]
Robot Arm 1 togoal: 19.3388507322924
Mobile Platform 2 Pose: [-9.00471728242907;12.9546504114254;0.530391080751273]
Robot Arm 2 end: [12.6960315594444;1.15355039253374;0.737535664936287]
Robot Arm 1 togoal: 25.3006986320616

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-8.77140702928151;-3.89068266640733;-4.05404946776968]
Robot Arm 1 end: [-3.79466101210802;-3.27103273846531;0.492880000069735]
Robot Arm 1 togoal: 19.16950737809
Mobile Platform 2 Pose: [-8.97719201136919;12.9387940562923;0.529750885436065]
Robot Arm 2 end: [12.5678835740991;1.09062444446529;0.782548899388139]
Robot Arm 1 togoal: 25.1566418587668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-8.65727512469872;-3.95710262529311;-3.98138128142042]
Robot Arm 1 end: [-4.06128858498043;-3.20684164666572;0.675563172892723]
Robot Arm 1 togoal: 19.3094605408549
Mobile Platform 2 Pose: [-8.94410349254462;12.9195462424608;0.529122564526456]
Robot Arm 2 end: [12.4503311986256;0.998173450527671;0.847571977769984]
Robot Arm 1 togoal: 25.0086124260624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-8.68855529801104;-3.9437489365173;-3.95554493004348]
Robot Arm 1 end: [-3.84672095447927;-3.17254647566086;0.493524727850436]
Robot Arm 1 togoal: 19.1390534582967
Mobile Platform 2 Pose: [-8.90228158071256;12.8948387554918;0.528408850652319]
Robot Arm 2 end: [12.3560019754943;0.882251449873237;0.921526477799558]
Robot Arm 1 togoal: 24.8711396894149

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-8.56918304926055;-4.01296489393808;-3.88681651246746]
Robot Arm 1 end: [-4.11692668681044;-3.11221339158486;0.675350434136392]
Robot Arm 1 togoal: 19.2855614980608
Mobile Platform 2 Pose: [-8.84324237842859;12.8590947498452;0.527484011242131]
Robot Arm 2 end: [12.2863166234904;0.761405791822289;1.0059165844452]
Robot Arm 1 togoal: 24.753849684114

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-8.59993013893014;-4.00001541051506;-3.86024494468515]
Robot Arm 1 end: [-3.90228930458022;-3.07722808043362;0.494154983874553]
Robot Arm 1 togoal: 19.1139203949129
Mobile Platform 2 Pose: [-8.74359344988639;12.7965102932807;0.526218186001796]
Robot Arm 2 end: [12.2281206539469;0.658917619760026;1.09933693008069]
Robot Arm 1 togoal: 24.6552159263671

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-8.47763181838782;-4.07076336615726;-3.79356340501677]
Robot Arm 1 end: [-4.17467932629105;-3.01892616713008;0.675163420641452]
Robot Arm 1 togoal: 19.2647193612403
Mobile Platform 2 Pose: [-8.56913676193318;12.6852827248741;0.523602263771524]
Robot Arm 2 end: [12.0244087199985;0.699783181689183;0.982560439725659]
Robot Arm 1 togoal: 24.4918324500388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-8.50755264670024;-4.05840538310035;-3.76615013141052]
Robot Arm 1 end: [-3.96018772746261;-2.98308797116722;0.494783926605892]
Robot Arm 1 togoal: 19.0918970791893
Mobile Platform 2 Pose: [-8.38519627073696;12.5676001123073;0.522585862508547]
Robot Arm 2 end: [11.833692321947;0.675189384034162;0.872148332114805]
Robot Arm 1 togoal: 24.3123589411098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.018759e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-8.38302635078782;-4.13029644824166;-3.7018613973183]
Robot Arm 1 end: [-4.23434262528794;-2.92721865498247;0.674994221207655]
Robot Arm 1 togoal: 19.2469534707591
Mobile Platform 2 Pose: [-8.1536952273937;12.4161233323149;0.522201118413101]
Robot Arm 2 end: [11.6378985391666;0.608702510687566;0.755085040555121]
Robot Arm 1 togoal: 24.1107828134351

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003299e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-8.41227872243973;-4.11842580316494;-3.67369421927074]
Robot Arm 1 end: [-4.01991678823268;-2.89056262311194;0.495399422722993]
Robot Arm 1 togoal: 19.0729265723289
Mobile Platform 2 Pose: [-7.78389219406034;12.1821595390478;0.52148254312422]
Robot Arm 2 end: [11.4090861297431;0.508034603011259;0.695204180833044]
Robot Arm 1 togoal: 23.8631452005764

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967381e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-8.28632775697885;-4.19102201215917;-3.61138620227985]
Robot Arm 1 end: [-4.29534309183581;-2.8367616081353;0.674834938647702]
Robot Arm 1 togoal: 19.2316218247044
Mobile Platform 2 Pose: [-7.49645202538951;11.996285339056;0.509070147451718]
Robot Arm 2 end: [11.2419987441365;0.564897961447658;0.592706521425877]
Robot Arm 1 togoal: 23.7423947478779

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000397e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-8.31494568281811;-4.17961404388068;-3.58250274083935]
Robot Arm 1 end: [-4.08097902892551;-2.7992819170192;0.49600151403586]
Robot Arm 1 togoal: 19.0563955117594
Mobile Platform 2 Pose: [-7.23759608468377;11.844241664534;0.492257526837426]
Robot Arm 2 end: [11.0966535898886;0.478820713567439;0.479315929594167]
Robot Arm 1 togoal: 23.57879517578

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.764765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-8.18812494927993;-4.25261302804481;-3.52197492037966]
Robot Arm 1 end: [-4.35733282241384;-2.74738886221455;0.674679887138238]
Robot Arm 1 togoal: 19.2183554528349
Mobile Platform 2 Pose: [-6.83841155035307;11.6291480121344;0.479901690613198]
Robot Arm 2 end: [10.8940851104006;0.491839713297895;0.498922947997618]
Robot Arm 1 togoal: 23.4026950300692

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.806356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-8.21616636666908;-4.24162765195122;-3.49242297352346]
Robot Arm 1 end: [-4.14300831830809;-2.70909640360245;0.496589000150704]
Robot Arm 1 togoal: 19.041933556874
Mobile Platform 2 Pose: [-6.61672499973335;11.4987351646658;0.467458373503434]
Robot Arm 2 end: [10.7825502561234;0.599682153704796;0.483165074916971]
Robot Arm 1 togoal: 23.352617802713

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.829662e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-8.08889793381336;-4.31480091411064;-3.43346267210624]
Robot Arm 1 end: [-4.42002364927233;-2.65893254672662;0.674524814107875]
Robot Arm 1 togoal: 19.2068262737101
Mobile Platform 2 Pose: [-6.35367147497055;11.3505035624053;0.452468569019716]
Robot Arm 2 end: [10.661962859794;0.518909001404996;0.345388587065263]
Robot Arm 1 togoal: 23.2152152260876

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.758130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-8.11641345875972;-4.30420277985555;-3.40328663474855]
Robot Arm 1 end: [-4.20571873033141;-2.61984065044428;0.497162321846681]
Robot Arm 1 togoal: 19.0292161236111
Mobile Platform 2 Pose: [-6.15020113611746;11.2292535208726;0.440225876905642]
Robot Arm 2 end: [10.5933415230633;0.432950350609644;0.262495577558687]
Robot Arm 1 togoal: 23.1196021048182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.827765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-7.98901313959978;-4.37738023858151;-3.34571263358957]
Robot Arm 1 end: [-4.48319328474629;-2.57125330627301;0.674366627739839]
Robot Arm 1 togoal: 19.1967727881752
Mobile Platform 2 Pose: [-6.04047221015675;11.1621656526774;0.432860240844537]
Robot Arm 2 end: [10.4799655905507;0.509199167670826;0.383474201829041]
Robot Arm 1 togoal: 23.0470436862826

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.926432e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-8.01604944162542;-4.36713665786098;-3.31495416511965]
Robot Arm 1 end: [-4.26888853005471;-2.53137747485615;0.497722552207335]
Robot Arm 1 togoal: 19.017983542837
Mobile Platform 2 Pose: [-5.87506463933538;11.0685028682311;0.419256917906514]
Robot Arm 2 end: [10.4902236772053;0.645725934861648;0.256935505265089]
Robot Arm 1 togoal: 23.1252806700576

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000104e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-7.88875503434471;-4.44019097177735;-3.25861034284155]
Robot Arm 1 end: [-4.54666710660541;-2.48423451503158;0.674203119233336]
Robot Arm 1 togoal: 19.1879832263315
Mobile Platform 2 Pose: [-5.65294706560212;10.943395175024;0.401549883606722]
Robot Arm 2 end: [10.3283927778749;0.476851021796311;0.241544298172935]
Robot Arm 1 togoal: 22.9051297127392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-7.91535407689398;-4.43027247363148;-3.22730754215946]
Robot Arm 1 end: [-4.33234451218145;-2.4435908319567;0.498271238053776]
Robot Arm 1 togoal: 19.008025308525
Mobile Platform 2 Pose: [-5.35672464726549;10.7674438907023;0.383239507166504]
Robot Arm 2 end: [10.136428331907;0.459716318747968;0.226143351020179]
Robot Arm 1 togoal: 22.7279137884698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.878283e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-7.7883460201896;-4.5031075655039;-3.17206109162658]
Robot Arm 1 end: [-4.61030694919805;-2.39777961089628;0.674032751355742]
Robot Arm 1 togoal: 19.180285763002
Mobile Platform 2 Pose: [-5.12344798089863;10.6207796673769;0.374685844065917]
Robot Arm 2 end: [9.99564042451006;0.421596345629051;0.22148328387037]
Robot Arm 1 togoal: 22.585925929061

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.856652e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-7.8145451142504;-4.49348791630984;-3.14024812116328]
Robot Arm 1 end: [-4.39595021829686;-2.3563837186403;0.498810197623145]
Robot Arm 1 togoal: 18.999170331413
Mobile Platform 2 Pose: [-4.9120370399916;10.4748682518533;0.375820484500085]
Robot Arm 2 end: [9.88339779414199;0.593626155991345;0.212653585910755]
Robot Arm 1 togoal: 22.5673701018141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.900472e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-7.68796176027734;-4.56603052594923;-3.08598672149344]
Robot Arm 1 end: [-4.67400238872887;-2.31180883772342;0.673854483229592]
Robot Arm 1 togoal: 19.1735400614401
Mobile Platform 2 Pose: [-4.69787202603839;10.3389247921025;0.375757393894552]
Robot Arm 2 end: [9.70062149892664;0.45782370405641;0.254745168566636]
Robot Arm 1 togoal: 22.3401872276672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005569e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-7.71379386976265;-4.55668657713986;-3.05369362564951]
Robot Arm 1 end: [-4.45959703075656;-2.26967521434783;0.499341408650012]
Robot Arm 1 togoal: 18.9912785887292
Mobile Platform 2 Pose: [-4.41821608355635;10.1411508937475;0.384219775113846]
Robot Arm 2 end: [9.56566341327489;0.632888918583539;0.204855827720063]
Robot Arm 1 togoal: 22.3070784066118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.894947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-7.58774264862677;-4.62888011171034;-3.0003225685937]
Robot Arm 1 end: [-4.73766418634618;-2.22625614520592;0.673667642422842]
Robot Arm 1 togoal: 19.1676304514119
Mobile Platform 2 Pose: [-4.23112065016567;10.0220508864794;0.386237179057567]
Robot Arm 2 end: [9.42132455969982;0.496991911841092;0.188387075916643]
Robot Arm 1 togoal: 22.1167480443191

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.871083e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-7.61323687127406;-4.61979152745037;-2.96757522914973]
Robot Arm 1 end: [-4.52319742816041;-2.18339761542338;0.499866919925575]
Robot Arm 1 togoal: 18.9842343474803
Mobile Platform 2 Pose: [-4.02348999223183;9.88592575822751;0.385735997830767]
Robot Arm 2 end: [9.30427930228868;0.477051622267973;0.168126930634544]
Robot Arm 1 togoal: 22.0057736882449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.849077e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-7.48780244290424;-4.69159158773138;-2.9150147375197]
Robot Arm 1 end: [-4.80121928332118;-2.14106641973709;0.673471825897622]
Robot Arm 1 togoal: 19.1624604392892
Mobile Platform 2 Pose: [-3.91650188718363;9.81932024707385;0.38162345613386]
Robot Arm 2 end: [9.26297970330756;0.462545849947539;0.141490467503929]
Robot Arm 1 togoal: 21.9642960110845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.851061e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-7.51298460058471;-4.68274049783726;-2.88183491030253]
Robot Arm 1 end: [-4.58667991969997;-2.09749382596795;0.50038881062562]
Robot Arm 1 togoal: 18.9779407510137
Mobile Platform 2 Pose: [-3.86730044188247;9.78949425271997;0.378752507013233]
Robot Arm 2 end: [9.21708217788209;0.480916668193184;0.152562412880529]
Robot Arm 1 togoal: 21.9321335730686

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.841051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-7.38823481395403;-4.75411162123418;-2.83001774414468]
Robot Arm 1 end: [-4.8646070137559;-2.05619309710455;0.673266833390063]
Robot Arm 1 togoal: 19.1579483554436
Mobile Platform 2 Pose: [-3.81680551937919;9.75880565374578;0.376148724142531]
Robot Arm 2 end: [9.15456459410181;0.525603727796738;0.192611474851078]
Robot Arm 1 togoal: 21.8963534566369

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.844884e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-7.41312808317833;-4.74548224584521;-2.79642313778194]
Robot Arm 1 end: [-4.64998517607857;-2.01191507813145;0.500909162687514]
Robot Arm 1 togoal: 18.9723154838816
Mobile Platform 2 Pose: [-3.76499070658032;9.72716014592205;0.374199593541003]
Robot Arm 2 end: [9.08188365472636;0.557487777018452;0.262715471369907]
Robot Arm 1 togoal: 21.8440406502414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.778215e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-7.28911836519264;-4.81639551628745;-2.74529253411495]
Robot Arm 1 end: [-4.92777614697119;-1.97159615115882;0.673052623167751]
Robot Arm 1 togoal: 19.1540238554305
Mobile Platform 2 Pose: [-3.70385038202346;9.68909473506603;0.373073303554123]
Robot Arm 2 end: [9.03965848688107;0.662706419950853;0.352710819831824]
Robot Arm 1 togoal: 21.8533018810344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.760039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-7.31374392103105;-4.80797378121996;-2.71129689969208]
Robot Arm 1 end: [-4.71306304769313;-1.92661899061844;0.501430044639058]
Robot Arm 1 togoal: 18.9672873043735
Mobile Platform 2 Pose: [-3.65721440593928;9.65995593474202;0.372972083569304]
Robot Arm 2 end: [9.07963853713927;0.7445019567094;0.311494879626187]
Robot Arm 1 togoal: 21.9303404360678

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.289793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.833903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-7.19051555434627;-4.87840754976728;-2.66080482709273]
Robot Arm 1 end: [-4.98992105984326;-1.8951679432613;0.75593388304815]
Robot Arm 1 togoal: 19.1514511184409
Mobile Platform 2 Pose: [-3.633487026291;9.64508424501206;0.373545170764081]
Robot Arm 2 end: [9.1038082849204;0.830486165738832;0.360668646956012]
Robot Arm 1 togoal: 21.9909365517276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.411718e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-7.21270271390965;-4.8698247925487;-2.63599548669056]
Robot Arm 1 end: [-4.77477961485013;-1.84095290536009;0.586961826742152]
Robot Arm 1 togoal: 18.9571808474638
Mobile Platform 2 Pose: [-3.61203763747907;9.63188537600173;0.374128791970518]
Robot Arm 2 end: [9.09461758110419;0.852728770214368;0.382225810393955]
Robot Arm 1 togoal: 21.9927931146402

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.104230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-7.04640542253972;-4.96494359411101;-2.5254062572444]
Robot Arm 1 end: [-5.079945353835;-1.76027860835164;0.755520312640526]
Robot Arm 1 togoal: 19.138831311563
Mobile Platform 2 Pose: [-3.58567549972959;9.61583537127736;0.374689687438266]
Robot Arm 2 end: [9.07588803004172;0.859680035232767;0.391108524434714]
Robot Arm 1 togoal: 21.9795088514024

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.873769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-7.08272289503998;-4.94739720470624;-2.50601115827335]
Robot Arm 1 end: [-4.85814526426273;-1.71024770744224;0.585794471348187]
Robot Arm 1 togoal: 18.9411982578825
Mobile Platform 2 Pose: [-3.55431959196756;9.59683861040378;0.375249044765901]
Robot Arm 2 end: [9.01815245632732;0.826070453389066;0.400954296657844]
Robot Arm 1 togoal: 21.9122893986783

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001853e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-6.98001032149685;-5.00688705001519;-2.36480772618074]
Robot Arm 1 end: [-4.93306740604912;-1.59460798801339;0.755173941420109]
Robot Arm 1 togoal: 18.9213616344414
Mobile Platform 2 Pose: [-3.51274668423291;9.5716149093793;0.375748329968592]
Robot Arm 2 end: [8.93302225742393;0.755684551884276;0.417543724824727]
Robot Arm 1 togoal: 21.8027910536925

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.943334e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.874387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-6.93492215970922;-5.03316471691387;-2.32994886595547]
Robot Arm 1 end: [-5.07260853006566;-1.52712660939618;0.597588601544854]
Robot Arm 1 togoal: 18.9976541809891
Mobile Platform 2 Pose: [-3.45351296781648;9.5354110840166;0.376035689260467]
Robot Arm 2 end: [8.83494121638917;0.652487356815248;0.440951124301076]
Robot Arm 1 togoal: 21.6655718384224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.863110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.765339e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-6.98134885506585;-5.0115620555515;-2.28388168414269]
Robot Arm 1 end: [-4.86457859428635;-1.49476351377597;0.731416777410017]
Robot Arm 1 togoal: 18.807145628424
Mobile Platform 2 Pose: [-3.36452956995209;9.48031103452308;0.375708320152009]
Robot Arm 2 end: [8.73481016195404;0.551480800472738;0.483846217441419]
Robot Arm 1 togoal: 21.5267867285361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.009717e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.526618e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-6.83106700244561;-5.09406822392001;-2.20510557848108]
Robot Arm 1 end: [-5.3251144639105;-1.43493521632979;0.603871877002218]
Robot Arm 1 togoal: 19.1430927680877
Mobile Platform 2 Pose: [-3.22553037821713;9.3923209791481;0.374592461092739]
Robot Arm 2 end: [8.6400897348105;0.541332989959801;0.493763118072175]
Robot Arm 1 togoal: 21.4389333255784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.637921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-6.86163440702611;-5.08001165661775;-2.19180605974449]
Robot Arm 1 end: [-5.11725314470081;-1.38644667955136;0.654930568255714]
Robot Arm 1 togoal: 18.9455162739851
Mobile Platform 2 Pose: [-3.09868607500688;9.31302060358245;0.372605709358937]
Robot Arm 2 end: [8.53685149014625;0.370014729465262;0.518368612829177]
Robot Arm 1 togoal: 21.2639915903765

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.819258e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-6.82146536790866;-5.11335103680106;-2.09005418672379]
Robot Arm 1 end: [-4.8088404044353;-1.35930241260343;0.805676810104639]
Robot Arm 1 togoal: 18.67745145064
Mobile Platform 2 Pose: [-2.86628089880522;9.16171476490416;0.365973114998031]
Robot Arm 2 end: [8.39131497870536;0.414832200795673;0.473210910114057]
Robot Arm 1 togoal: 21.1614521806413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001026e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.869577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-6.7557437806417;-5.14851359417289;-2.07614656197688]
Robot Arm 1 end: [-5.04531140449467;-1.27871835208766;0.702990992662252]
Robot Arm 1 togoal: 18.8212651450433
Mobile Platform 2 Pose: [-2.70925312004035;9.05962628296485;0.363247713243293]
Robot Arm 2 end: [8.29040317611391;0.250417851889395;0.609056548221006]
Robot Arm 1 togoal: 20.9866994866361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002013e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.212071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-6.61550581072502;-5.22758179959141;-1.96842918658385]
Robot Arm 1 end: [-5.23614853883571;-1.15791445867028;0.54986668934698]
Robot Arm 1 togoal: 18.9098486845223
Mobile Platform 2 Pose: [-2.46004464294793;8.8859641212557;0.3564163540152]
Robot Arm 2 end: [8.14532143127587;0.210628617767131;0.425494090317542]
Robot Arm 1 togoal: 20.849694477676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.631036e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.530000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-6.66340332469171;-5.20266299656967;-1.93836902241985]
Robot Arm 1 end: [-5.06975986544483;-1.15043912689621;0.699243280663655]
Robot Arm 1 togoal: 18.7644674646142
Mobile Platform 2 Pose: [-2.22967111687203;8.72650146026276;0.347622394863869]
Robot Arm 2 end: [7.9523878784817;0.353206870139277;0.667792336936123]
Robot Arm 1 togoal: 20.7413858348874

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000421e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.101910e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-6.5573269701862;-5.26071160965144;-1.86380563887708]
Robot Arm 1 end: [-5.34241468380974;-1.0523685186278;0.579496023437062]
Robot Arm 1 togoal: 18.9322731846817
Mobile Platform 2 Pose: [-2.10464853677959;8.64406766597669;0.345322391229916]
Robot Arm 2 end: [7.88346715161528;0.231568475902016;0.494362015973459]
Robot Arm 1 togoal: 20.6290420618656

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.846209e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.309437e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-6.5968214139751;-5.24100593842125;-1.84291911647051]
Robot Arm 1 end: [-5.15085142017818;-1.04791081119325;0.740022892361245]
Robot Arm 1 togoal: 18.7673801392483
Mobile Platform 2 Pose: [-1.84980742957731;8.47543295096617;0.334927484615322]
Robot Arm 2 end: [7.74152892701604;0.0780672373230658;0.640640362704566]
Robot Arm 1 togoal: 20.4231320075822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.046504e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.574664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-6.51036457620779;-5.29032022553364;-1.75158486975332]
Robot Arm 1 end: [-5.31678629326462;-0.926134336416547;0.577695943500779]
Robot Arm 1 togoal: 18.8381043747498
Mobile Platform 2 Pose: [-1.59989156284986;8.29404486641231;0.321003556525913]
Robot Arm 2 end: [7.51840144036687;0.191860952413254;0.48594486223155]
Robot Arm 1 togoal: 20.2938354639382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.908865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.140931e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-6.55220932849338;-5.26839857073958;-1.73098764482811]
Robot Arm 1 end: [-5.16772700270342;-0.93866090293447;0.753033759014968]
Robot Arm 1 togoal: 18.7164103643091
Mobile Platform 2 Pose: [-1.39830202431015;8.1599900007149;0.312553506750942]
Robot Arm 2 end: [7.38724407546578;0.147619398148447;0.683429932984235]
Robot Arm 1 togoal: 20.1492421596113

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.026595e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-6.53827275954238;-5.27549247029082;-1.61898098163138]
Robot Arm 1 end: [-5.27417165279995;-0.823376213281882;0.41446554219655]
Robot Arm 1 togoal: 18.7528720690922
Mobile Platform 2 Pose: [-1.13611870282133;7.98511214700251;0.299959781870215]
Robot Arm 2 end: [7.22022722539901;0.0759971727650998;0.495355140706981]
Robot Arm 1 togoal: 19.9778350202753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.840671e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.862912e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-6.55719545600968;-5.26565112208496;-1.60513070343176]
Robot Arm 1 end: [-5.3398909082882;-0.80465191830557;0.573152134315953]
Robot Arm 1 togoal: 18.7869947498372
Mobile Platform 2 Pose: [-0.917841433236376;7.84314649594169;0.289621206932612]
Robot Arm 2 end: [7.04812753910391;0.20649051276593;0.64494756308708]
Robot Arm 1 togoal: 19.8891570723587

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.987782e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.869056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-6.63043585638784;-5.23018409981759;-1.56560277491887]
Robot Arm 1 end: [-5.11278779536395;-0.785190471862446;0.67758458439652]
Robot Arm 1 togoal: 18.5856944456034
Mobile Platform 2 Pose: [-0.704991262579394;7.71523609010723;0.280968093182719]
Robot Arm 2 end: [6.98647841269921;-0.0142350247793791;0.521109665026919]
Robot Arm 1 togoal: 19.7295649137889

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.030107e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.948396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-6.590320130403;-5.25359107839802;-1.52646147923612]
Robot Arm 1 end: [-5.3443919619522;-0.724510593059414;0.561523162196706]
Robot Arm 1 togoal: 18.7452950882334
Mobile Platform 2 Pose: [-0.436162034482391;7.53318407585986;0.262140960769599]
Robot Arm 2 end: [6.77249514835279;0.0433645655821142;0.689107690278329]
Robot Arm 1 togoal: 19.5672637359873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007496e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-6.65271480580036;-5.22324547581259;-1.49602202159278]
Robot Arm 1 end: [-5.11179044077478;-0.713383486407668;0.677382116582679]
Robot Arm 1 togoal: 18.5433138135002
Mobile Platform 2 Pose: [-0.209583716023195;7.38794519584688;0.246919409874805]
Robot Arm 2 end: [6.71313044504295;-0.176741622919473;0.528047088559921]
Robot Arm 1 togoal: 19.4116225468208

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.045599e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.662767e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-6.62134388812544;-5.24113102224089;-1.43841200298451]
Robot Arm 1 end: [-5.33707712423916;-0.628620867850538;0.564790925437789]
Robot Arm 1 togoal: 18.6844280635411
Mobile Platform 2 Pose: [-0.0443652030478195;7.2666817262318;0.229105990364849]
Robot Arm 2 end: [6.51662025392681;-0.00649552882253945;0.691517561488553]
Robot Arm 1 togoal: 19.322445946196

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015314e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.025166e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-6.67737175258679;-5.21260469234787;-1.41932532383654]
Robot Arm 1 end: [-5.10645754963871;-0.627330322198651;0.679399910336877]
Robot Arm 1 togoal: 18.4892806833333
Mobile Platform 2 Pose: [0.159019451948326;7.13571043049106;0.215026995458435]
Robot Arm 2 end: [6.47007811658908;-0.221861320042051;0.535329509908094]
Robot Arm 1 togoal: 19.1793382452595

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.817318e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.810200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-6.74292150059403;-5.174579470753;-1.29730351784787]
Robot Arm 1 end: [-4.97030039195127;-0.503033039683057;0.568245401921592]
Robot Arm 1 togoal: 18.3120544500635
Mobile Platform 2 Pose: [0.365500346966722;6.98544738752164;0.191825238608972]
Robot Arm 2 end: [6.24246132354414;-0.0600472924442745;0.693460120124047]
Robot Arm 1 togoal: 19.0605622143749

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.757304e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.035848e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-6.79216783191365;-5.14298581527532;-1.25420928412431]
Robot Arm 1 end: [-4.92550851014963;-0.541965457562557;0.931631213914353]
Robot Arm 1 togoal: 18.2825379956122
Mobile Platform 2 Pose: [0.548728527106409;6.86829104822209;0.178408626645634]
Robot Arm 2 end: [6.21523405366029;-0.269769953584203;0.538769851426736]
Robot Arm 1 togoal: 18.9361022450747

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.428502e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999603e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-6.85596437956346;-5.1034230701781;-1.22572180817082]
Robot Arm 1 end: [-4.680856989016;-0.534398388202469;0.765238440548325]
Robot Arm 1 togoal: 18.0851399460454
Mobile Platform 2 Pose: [0.809345884728061;6.68072748523883;0.148386243567686]
Robot Arm 2 end: [5.9473759853509;-0.118923266520961;0.695834840277953]
Robot Arm 1 togoal: 18.7785882262716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-6.85311171235565;-5.08984318302776;-1.16988872907826]
Robot Arm 1 end: [-4.87214686865945;-0.441993194118512;0.923211391203419]
Robot Arm 1 togoal: 18.1816755435191
Mobile Platform 2 Pose: [0.973980682187123;6.57642461363471;0.135862906205961]
Robot Arm 2 end: [5.93582189950258;-0.32307198600738;0.541412700465038]
Robot Arm 1 togoal: 18.669572750282

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998158e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-6.97457672820298;-5.01621827221534;-1.12992652810089]
Robot Arm 1 end: [-4.58260082267789;-0.431098573388398;0.751472946402209]
Robot Arm 1 togoal: 17.9458138875764
Mobile Platform 2 Pose: [1.28897102937894;6.3539642856961;0.098356549804069]
Robot Arm 2 end: [5.7329843873278;-0.369758145010159;0.697293053147017]
Robot Arm 1 togoal: 18.4647461139273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-6.96089090158993;-5.01266456439651;-1.08886414012857]
Robot Arm 1 end: [-4.79020055952231;-0.362035944057615;0.922374781129407]
Robot Arm 1 togoal: 18.0687623718257
Mobile Platform 2 Pose: [1.47876794080324;6.22949739747503;0.0745710485032164]
Robot Arm 2 end: [5.75067941812507;-0.553053110130333;0.542995889392233]
Robot Arm 1 togoal: 18.3925310850066

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994684e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-7.08129415931726;-4.93931562251116;-1.04684751593078]
Robot Arm 1 end: [-4.50113244673588;-0.351040488307849;0.751883786329745]
Robot Arm 1 togoal: 17.8330709968445
Mobile Platform 2 Pose: [1.75459316765066;6.02993466389588;0.0125591408258115]
Robot Arm 2 end: [5.46287591759277;-0.51848761652285;0.711642874254988]
Robot Arm 1 togoal: 18.1563946773002

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.032960e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-7.05669451981885;-4.9424384025302;-1.00778597836572]
Robot Arm 1 end: [-4.71533250120448;-0.282074188750511;0.921676932276813]
Robot Arm 1 togoal: 17.9616605129782
Mobile Platform 2 Pose: [1.89397723476297;5.94235830473927;-0.00930696373734111]
Robot Arm 2 end: [5.52938768400815;-0.685056419234159;0.558008915330335]
Robot Arm 1 togoal: 18.1344260339893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.951546e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-7.17526548732067;-4.869768043052;-0.963490032661574]
Robot Arm 1 end: [-4.42717606793712;-0.270674406587755;0.75247294642614]
Robot Arm 1 togoal: 17.7262741684052
Mobile Platform 2 Pose: [2.14732437112954;5.75570103923012;-0.0839710063735154]
Robot Arm 2 end: [5.31111884921017;-0.720248536979258;0.726916617482993]
Robot Arm 1 togoal: 17.9213480533264

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.032171e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-7.14090495953623;-4.87894396504483;-0.926508766309873]
Robot Arm 1 end: [-4.64737173841905;-0.201947934632404;0.921125549413581]
Robot Arm 1 togoal: 17.860153690852
Mobile Platform 2 Pose: [2.27281182256531;5.67670220549327;-0.112178181569135]
Robot Arm 2 end: [5.41118418647788;-0.862009955331584;0.576592325609709]
Robot Arm 1 togoal: 17.9415541673917

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999429e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-7.25686674651897;-4.8073624352889;-0.879743922439274]
Robot Arm 1 end: [-4.36055591973579;-0.189861301677765;0.753241115853512]
Robot Arm 1 togoal: 17.6252175412241
Mobile Platform 2 Pose: [2.44312107030016;5.54482455870436;-0.201891106394793]
Robot Arm 2 end: [5.11917086380726;-0.808059086143917;0.863512984723198]
Robot Arm 1 togoal: 17.7063094558995

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012272e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-7.21411977619217;-4.82181972860187;-0.844839669802796]
Robot Arm 1 end: [-4.58599698966168;-0.121448208169787;0.920719535587492]
Robot Arm 1 togoal: 17.7638779169707
Mobile Platform 2 Pose: [2.50088546309483;5.51104681209364;-0.214339856012629]
Robot Arm 2 end: [5.21769139705927;-0.948160229627347;0.722899086837368]
Robot Arm 1 togoal: 17.7243025180745

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.041070e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-7.326751494962;-4.75171109981148;-0.795462431424061]
Robot Arm 1 end: [-4.30091224826731;-0.1084297617875;0.754181838821883]
Robot Arm 1 togoal: 17.5295308971446
Mobile Platform 2 Pose: [2.66966612037918;5.40284016902604;-0.277168150476941]
Robot Arm 2 end: [5.30428717469053;-1.08131165800315;0.575084628266334]
Robot Arm 1 togoal: 17.7386291341115

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001474e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-7.28175593944856;-4.76685574366477;-0.75885769804404]
Robot Arm 1 end: [-4.52704777587328;-0.0366317103964012;0.920447620906975]
Robot Arm 1 togoal: 17.6671943537358
Mobile Platform 2 Pose: [2.67856001874813;5.3841877309839;-0.314160045644416]
Robot Arm 2 end: [5.10660301194774;-1.05294322415334;0.728147030597486]
Robot Arm 1 togoal: 17.5752068363904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.038087e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-7.39703838056085;-4.6951904573698;-0.710271566763767]
Robot Arm 1 end: [-4.24066847470869;-0.0259518476299536;0.75521869649]
Robot Arm 1 togoal: 17.4328072914939
Mobile Platform 2 Pose: [2.84514645429388;5.28051408000022;-0.376949483974159]
Robot Arm 2 end: [5.15021795354706;-1.18092040356376;0.579884792647879]
Robot Arm 1 togoal: 17.5553686034149

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.718032e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013699e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-7.34963304354321;-4.71124035588453;-0.672398476124443]
Robot Arm 1 end: [-4.58046437843588;0.0768436219005929;0.921699328242923]
Robot Arm 1 togoal: 17.6470432449808
Mobile Platform 2 Pose: [2.86771711377777;5.25435777227337;-0.414835159043362]
Robot Arm 2 end: [4.94411214319988;-1.11727045447707;0.850173933843604]
Robot Arm 1 togoal: 17.3976831658263

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.128261e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.038655e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-7.44120845717385;-4.65810261930502;-0.642331405717289]
Robot Arm 1 end: [-4.31038170672185;0.101546541372953;0.757507354986262]
Robot Arm 1 togoal: 17.4169264361906
Mobile Platform 2 Pose: [2.99567494745796;5.17101238057819;-0.458687386124977]
Robot Arm 2 end: [4.72615870121763;-1.14286257039359;0.710167143809178]
Robot Arm 1 togoal: 17.2036648264566

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.597350e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.877254e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-7.40002060631013;-4.66992290879156;-0.582118253898892]
Robot Arm 1 end: [-4.62731560909846;0.132399028609333;1.01432490971521]
Robot Arm 1 togoal: 17.6517599832391
Mobile Platform 2 Pose: [3.12184223179149;5.09338538931251;-0.487202789442656]
Robot Arm 2 end: [4.83813585849645;-1.23252288164401;0.858329182984793]
Robot Arm 1 togoal: 17.2475523883193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.251939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-7.45409605448812;-4.63953128079814;-0.566146815992832]
Robot Arm 1 end: [-4.387912781734;0.1877810731249;0.859682500612919]
Robot Arm 1 togoal: 17.4278230229339
Mobile Platform 2 Pose: [3.28568372507689;4.96164661482103;-0.576005168011436]
Robot Arm 2 end: [4.51403265429048;-1.23244943050419;0.703101961730485]
Robot Arm 1 togoal: 16.9763377699515

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.922543e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-7.52278639563948;-4.57991405598461;-0.393609179675109]
Robot Arm 1 end: [-4.49090658661053;0.401483942940364;0.778560282476455]
Robot Arm 1 togoal: 17.3973877617779
Mobile Platform 2 Pose: [3.42887934215916;4.87499406890467;-0.606655432780317]
Robot Arm 2 end: [4.61047821607996;-1.3745638447784;0.557491511169178]
Robot Arm 1 togoal: 16.9938980571969
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 90)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',90,0)">第 90 行</a>)
    real_time_2D_plot(human_poses{k-1}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {1×7 cell}
              M: {1×7 cell}
     joint_base: {1×7 cell}
    joint_world: {1×7 cell}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.884435e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785228159289654;-9.9998844326399;-9.75000002671158]
Robot Arm 1 end: [-10.2590260100714;-9.7535517369108;1.47381471675359]
Robot Arm 1 togoal: 28.295461960366
Mobile Platform 2 Pose: [-2.3506080128396;9.99619480583496;7.31185698040774]
Robot Arm 2 end: [10.2551426482981;7.30122746100388;1.47381471675359]
Robot Arm 1 togoal: 26.6384215337781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.413543008197901;-9.74991229854279;-9.74626722839916]
Robot Arm 1 end: [-10.1875803705378;-9.63342511507218;1.3825988352036]
Robot Arm 1 togoal: 28.1607694825353
Mobile Platform 2 Pose: [-1.82672406331166;9.63711075514008;7.30713017566845]
Robot Arm 2 end: [10.2774493143352;7.06509608900497;0.203883750145304]
Robot Arm 1 togoal: 26.5353961801597

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.089700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.380708377089659;-9.71840118780857;-9.76078650336839]
Robot Arm 1 end: [-10.1850026522751;-9.63147941850473;1.37049648064301]
Robot Arm 1 togoal: 28.1576271646044
Mobile Platform 2 Pose: [-1.88190242435854;9.66108794425246;7.29793351685333]
Robot Arm 2 end: [10.3903462024012;6.91486427417997;0.685774139801531]
Robot Arm 1 togoal: 26.5061587919723

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.280120e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.139303032026906;-9.48784127196464;-9.85744117323001]
Robot Arm 1 end: [-9.9794158886142;-9.6541560383201;1.32654862316824]
Robot Arm 1 togoal: 28.0267857623167
Mobile Platform 2 Pose: [-1.98918452304916;9.71456574966837;7.28488005508751]
Robot Arm 2 end: [10.1674544309343;7.26749967972952;1.30770366502666]
Robot Arm 1 togoal: 26.5506734140546

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.115647e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0297905183443635;-9.42046644012155;-9.85547286151884]
Robot Arm 1 end: [-9.86811389028013;-9.53577380989645;1.28280621336205]
Robot Arm 1 togoal: 27.864765281419
Mobile Platform 2 Pose: [-2.58313921762017;9.95449053707943;7.10951307286554]
Robot Arm 2 end: [9.38630172853247;7.43530754760035;0.226076300613866]
Robot Arm 1 togoal: 26.1054682274872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.179814241038298;-9.29089524215855;-9.85870051111235]
Robot Arm 1 end: [-9.61250463590919;-9.6486555903653;1.39882570484487]
Robot Arm 1 togoal: 27.762113232979
Mobile Platform 2 Pose: [-2.25769532312358;9.77741353129534;7.07869952723356]
Robot Arm 2 end: [9.31646211716613;6.67096749926344;0.220768717043397]
Robot Arm 1 togoal: 25.5487273161422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.196771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.638226951757348;-9.04282291842459;-9.82771445144579]
Robot Arm 1 end: [-9.35422363634081;-9.70485711899208;1.44651286649513]
Robot Arm 1 togoal: 27.6201541447263
Mobile Platform 2 Pose: [-2.16584540735235;9.72400767729413;7.08901764338485]
Robot Arm 2 end: [9.44738270050309;6.56505907059027;0.228744776698211]
Robot Arm 1 togoal: 25.578737253315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.733888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-1.08910938876278;-8.81036385297783;-9.73572105614528]
Robot Arm 1 end: [-9.0799598552878;-9.71318946732096;1.47843385132253]
Robot Arm 1 togoal: 27.4345861811492
Mobile Platform 2 Pose: [-2.01771989643655;9.6763372260804;7.04797076976288]
Robot Arm 2 end: [9.55906297871408;6.82323312089555;-0.0481091926346054]
Robot Arm 1 togoal: 25.8464740516712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-1.29637048098427;-8.5830884135062;-9.83986458505127]
Robot Arm 1 end: [-8.90656976314026;-9.71758857759475;1.44579043441326]
Robot Arm 1 togoal: 27.3175279994831
Mobile Platform 2 Pose: [-2.01902472664504;9.72205436464418;6.93803828305495]
Robot Arm 2 end: [9.82720941930307;6.91036413363938;1.13779632192311]
Robot Arm 1 togoal: 26.0619549064929

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.632498e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-1.68378871511155;-8.3627945043235;-9.78308363577894]
Robot Arm 1 end: [-8.63895774274928;-9.76975946684155;1.47530758743927]
Robot Arm 1 togoal: 27.1708708529802
Mobile Platform 2 Pose: [-2.05609137425549;9.71005445677099;6.76939506557452]
Robot Arm 2 end: [9.12372844023708;6.30844874205583;0.264548592039329]
Robot Arm 1 togoal: 25.1646808040309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.195767e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-1.96519769690692;-8.11726415912372;-9.83014582430116]
Robot Arm 1 end: [-8.46308351931939;-9.82938877767146;1.44579043604994]
Robot Arm 1 togoal: 27.0942017563891
Mobile Platform 2 Pose: [-1.69977866122443;9.46795607145694;6.7070349260628]
Robot Arm 2 end: [9.02228989246621;6.07167387223347;0.35497606482086]
Robot Arm 1 togoal: 24.930018564491

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.548565e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-1.9969192732093;-8.10168060631298;-9.82149385992331]
Robot Arm 1 end: [-8.21001501597026;-9.81253731538611;1.50054826866843]
Robot Arm 1 togoal: 26.9098810853698
Mobile Platform 2 Pose: [-1.16019111802421;9.12737238995689;6.54901504168747]
Robot Arm 2 end: [9.21591628516431;6.84303186150692;-0.106063739653571]
Robot Arm 1 togoal: 25.6044233947784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.255202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-2.37186008637259;-7.85297148072931;-9.79612122008265]
Robot Arm 1 end: [-7.90822660710068;-9.78755703548972;1.51620874991695]
Robot Arm 1 togoal: 26.688050071148
Mobile Platform 2 Pose: [-0.497816629155066;8.74496972825144;6.48180490101546]
Robot Arm 2 end: [8.71735706189825;6.10237936699951;0.399463045131942]
Robot Arm 1 togoal: 24.7160660664779

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.938322e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-2.69071015469443;-7.62252572682795;-9.8930477619075]
Robot Arm 1 end: [-7.76347713843514;-9.9516238133049;1.49332851773394]
Robot Arm 1 togoal: 26.7134643681098
Mobile Platform 2 Pose: [-0.320733230281595;8.51274055159442;6.57437801396973]
Robot Arm 2 end: [8.35102375731321;6.2855903201096;0.456209529909296]
Robot Arm 1 togoal: 24.5583856098302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.692276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-3.00561232618968;-7.37469491477929;-9.86018610338833]
Robot Arm 1 end: [-7.32041432034957;-9.83050129571965;1.48488271402997]
Robot Arm 1 togoal: 26.3295810511002
Mobile Platform 2 Pose: [-0.0324773964748299;8.10153599427674;6.60323536927293]
Robot Arm 2 end: [7.80243170544126;6.40747410262793;0.441311107593431]
Robot Arm 1 togoal: 24.2342218599998

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.292776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-3.38040512888662;-7.1248557161461;-9.86915130824835]
Robot Arm 1 end: [-6.8972152321272;-9.75241160351292;1.41855687855988]
Robot Arm 1 togoal: 25.9939250854609
Mobile Platform 2 Pose: [0.659843019512591;7.60951852048598;6.56311444874536]
Robot Arm 2 end: [7.32380671458491;7.08652896713242;0.31142760612949]
Robot Arm 1 togoal: 24.362405804948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.585292e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-3.5732462814525;-6.99590962695865;-9.85492895946954]
Robot Arm 1 end: [-6.59601844522779;-9.78020955003642;1.30789996963712]
Robot Arm 1 togoal: 25.821114029039
Mobile Platform 2 Pose: [0.71889813125557;7.5720497807119;6.57509383115961]
Robot Arm 2 end: [7.01932573174459;7.01745742793466;0.399921423674435]
Robot Arm 1 togoal: 24.0937361745556

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.453888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-3.69414425395755;-6.90739589415052;-9.8398603469137]
Robot Arm 1 end: [-6.39044672996382;-9.84651645223196;1.16085622781983]
Robot Arm 1 togoal: 25.7421961540412
Mobile Platform 2 Pose: [1.66439320761248;7.02939737484906;6.59597582315264]
Robot Arm 2 end: [6.40495315979914;6.82979661710781;0.308691143980695]
Robot Arm 1 togoal: 23.5336829907858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.078992e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-3.80703661031281;-6.81207361677582;-9.81988303199898]
Robot Arm 1 end: [-6.2218079132518;-9.87365897323334;0.965162973640331]
Robot Arm 1 togoal: 25.6596634381414
Mobile Platform 2 Pose: [2.63795548564403;6.48113152864948;6.65746053033644]
Robot Arm 2 end: [5.86089944706849;6.74170859006395;0.220656962245099]
Robot Arm 1 togoal: 23.0985677001947

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.942016e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-3.91664596631455;-6.69737874790763;-9.78745635718539]
Robot Arm 1 end: [-6.09786817118789;-9.89388172409664;0.763859703125376]
Robot Arm 1 togoal: 25.6023661238262
Mobile Platform 2 Pose: [3.66621625059528;5.92192598490534;6.70253569578916]
Robot Arm 2 end: [5.24273646681534;6.93126265071003;0.39584819694121]
Robot Arm 1 togoal: 22.809529511238
Step 23:
Mobile Platform 1 Pose: [-4.09250206518483;-6.53925473845061;-9.76318688484551]
Robot Arm 1 end: [-5.90007303736337;-9.92948495675877;0.652683999295251]
Robot Arm 1 togoal: 25.5098150897305
Mobile Platform 2 Pose: [4.17269180061239;5.6719262578988;6.70290541334009]
Robot Arm 2 end: [4.96710582742084;6.73588577573033;0.300210373020706]
Robot Arm 1 togoal: 22.4854276557251
Step 24:
Mobile Platform 1 Pose: [-4.20047534566452;-6.45322565980037;-9.75941386163487]
Robot Arm 1 end: [-5.75753074263681;-9.72712800151692;0.544511543967333]
Robot Arm 1 togoal: 25.2668336757525
Mobile Platform 2 Pose: [4.53396369548231;5.42307803703215;6.7268754575956]
Robot Arm 2 end: [4.67468778763128;6.74699910096184;0.487310230207288]
Robot Arm 1 togoal: 22.2907369795672
Step 25:
Mobile Platform 1 Pose: [-3.77179022530771;-6.65989945136894;-9.69714899185911]
Robot Arm 1 end: [-6.00185214515853;-9.46851618895142;0.733155049644001]
Robot Arm 1 togoal: 25.213153201073
Mobile Platform 2 Pose: [5.05203023656603;5.1732056334204;6.71888909964637]
Robot Arm 2 end: [4.4703217982975;6.76004428541799;0.285410912015761]
Robot Arm 1 togoal: 22.1769244438854
Step 26:
Mobile Platform 1 Pose: [-3.7020757490421;-6.54566489833512;-9.49268739246742]
Robot Arm 1 end: [-5.96217571242511;-9.10219227827227;0.681171059718209]
Robot Arm 1 togoal: 24.9076316810602
Mobile Platform 2 Pose: [5.57332214954753;4.92347018097441;6.70739110888183]
Robot Arm 2 end: [4.17830314192265;6.70342585317396;0.47360368401855]
Robot Arm 1 togoal: 21.9345789624389
Step 27:
Mobile Platform 1 Pose: [-3.97254601983732;-6.32930879517412;-9.36742751610756]
Robot Arm 1 end: [-5.72760896566378;-8.92357255234198;0.571708183259536]
Robot Arm 1 togoal: 24.6243870354961
Mobile Platform 2 Pose: [6.11485203747584;4.67406996396344;6.69008412315122]
Robot Arm 2 end: [3.97194542676948;6.73101248260921;0.283615948564096]
Robot Arm 1 togoal: 21.8328458289935
Step 28:
Mobile Platform 1 Pose: [-4.61525911706625;-6.09234689657924;-9.28774653585309]
Robot Arm 1 end: [-5.35555795595469;-9.02975514633031;0.492569962738558]
Robot Arm 1 togoal: 24.4741171323811
Mobile Platform 2 Pose: [6.64955279181804;4.42513936612575;6.66698526040527]
Robot Arm 2 end: [3.68200491686588;6.63733237957624;0.472233409147306]
Robot Arm 1 togoal: 21.5661308216316
Step 29:
Mobile Platform 1 Pose: [-4.68885278606461;-5.88368342347059;-9.15005283517642]
Robot Arm 1 end: [-5.21577904933386;-8.75017784131592;0.686702514363549]
Robot Arm 1 togoal: 24.1616463316071
Mobile Platform 2 Pose: [7.15780631911457;4.17537613686615;6.65610733275951]
Robot Arm 2 end: [3.47586634552225;6.66672133160486;0.278010323440317]
Robot Arm 1 togoal: 21.4691312596431
Step 30:
Mobile Platform 1 Pose: [-4.68862977707429;-5.69542720848258;-8.98555481841515]
Robot Arm 1 end: [-5.12894611324176;-8.47779048219934;0.589633643024865]
Robot Arm 1 togoal: 23.8993973750874
Mobile Platform 2 Pose: [7.67621926194213;3.92580016096807;6.6415528745229]
Robot Arm 2 end: [3.18486698208387;6.59021430433166;0.47198056138557]
Robot Arm 1 togoal: 21.2173601039688
Step 31:
Mobile Platform 1 Pose: [-5.23089017331349;-5.46222671120191;-8.89545749288197]
Robot Arm 1 end: [-4.82342421120092;-8.51895851112919;0.399397575046908]
Robot Arm 1 togoal: 23.7474992721718
Mobile Platform 2 Pose: [8.15350674362266;3.67587603887443;6.63539388144454]
Robot Arm 2 end: [2.9781965084262;6.62004284481829;0.276246081823937]
Robot Arm 1 togoal: 21.1236554860123
Step 32:
Mobile Platform 1 Pose: [-5.22917307932815;-5.29819135106435;-8.70679800941892]
Robot Arm 1 end: [-4.7314145064307;-8.19955605004817;0.588328533594892]
Robot Arm 1 togoal: 23.4330597627234
Mobile Platform 2 Pose: [8.68172871287499;3.42694225297254;6.61232940146866]
Robot Arm 2 end: [2.69088079175024;6.53177193990416;0.47167532681261]
Robot Arm 1 togoal: 20.8676548997049
Step 33:
Mobile Platform 1 Pose: [-5.30689636527638;-5.13766229828106;-8.51514618110321]
Robot Arm 1 end: [-4.67797867524298;-7.93332912740079;0.399397575046908]
Robot Arm 1 togoal: 23.2014082241193
Mobile Platform 2 Pose: [9.108169470745;3.17694258524475;6.61192161701141]
Robot Arm 2 end: [2.48216678318398;6.5599141717305;0.27767967228607]
Robot Arm 1 togoal: 20.7745298482447

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-06','1.278068e-15','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-5.74241953032686;-4.96685402300467;-8.33259609729847]
Robot Arm 1 end: [-4.39199352265148;-7.83446177409713;0.589030022738427]
Robot Arm 1 togoal: 22.9361302831644
Mobile Platform 2 Pose: [9.6051274200331;2.92732974310654;6.59801374011584]
Robot Arm 2 end: [2.1993444266892;6.49207814645199;0.460732898119974]
Robot Arm 1 togoal: 20.5411005634314
Step 35:
Mobile Platform 1 Pose: [-5.76649253965448;-4.81482987678075;-8.13413033459601]
Robot Arm 1 end: [-4.34330151853458;-7.56126172102576;0.400851085895304]
Robot Arm 1 togoal: 22.7020471604596
Mobile Platform 2 Pose: [10.013046688914;2.67733227008181;6.59688969329711]
Robot Arm 2 end: [1.98785115234183;6.52862543957866;0.272371238004054]
Robot Arm 1 togoal: 20.4563708403488
Step 36:
Mobile Platform 1 Pose: [-6.21378062076851;-4.63883423295485;-7.95657602354765]
Robot Arm 1 end: [-4.05046141583389;-7.47389677974117;0.593658284903929]
Robot Arm 1 togoal: 22.4413122905824
Mobile Platform 2 Pose: [10.5044460646953;2.42771615739926;6.58304063956626]
Robot Arm 2 end: [1.7050069062558;6.45140736254555;0.467186148596967]
Robot Arm 1 togoal: 20.2179750350356
Step 37:
Mobile Platform 1 Pose: [-6.20187855132179;-4.49733089625943;-7.75047700437317]
Robot Arm 1 end: [-4.0079433055813;-7.18689510942763;0.411266181116236]
Robot Arm 1 togoal: 22.2000754121156
Mobile Platform 2 Pose: [10.9325741118157;2.17781097906659;6.57615573993296]
Robot Arm 2 end: [1.48926502440663;6.47608258004326;0.281376363928723]
Robot Arm 1 togoal: 20.1246409842403
Step 38:
Mobile Platform 1 Pose: [-6.70518112895723;-4.30134419275058;-7.59527000695432]
Robot Arm 1 end: [-3.89343755749005;-6.95098148307977;0.624004042963715]
Robot Arm 1 togoal: 21.9355415285451
Mobile Platform 2 Pose: [11.4104840260293;1.92817849881716;6.56260491691223]
Robot Arm 2 end: [1.2108530514802;6.40504358531011;0.480254871405652]
Robot Arm 1 togoal: 19.8970307232135
Step 39:
Mobile Platform 1 Pose: [-7.44449965033343;-4.06344276624289;-7.51843954871102]
Robot Arm 1 end: [-3.80298740666893;-6.81428764417648;0.419845004937809]
Robot Arm 1 togoal: 21.781994230394
Mobile Platform 2 Pose: [12.0389240348713;1.68048508006723;6.52872320473165]
Robot Arm 2 end: [0.999520627672858;6.53769641953227;0.304663869828107]
Robot Arm 1 togoal: 19.8988524916582
Step 40:
Mobile Platform 1 Pose: [-8.06370956526678;-3.84411501890025;-7.39845895494513]
Robot Arm 1 end: [-3.38567208455942;-6.73151434920032;0.63137284944704]
Robot Arm 1 togoal: 21.4455409360513
Mobile Platform 2 Pose: [12.7741114207914;1.43234357254757;6.49829637042377]
Robot Arm 2 end: [0.728255297614455;6.34394568425375;0.420348314247477]
Robot Arm 1 togoal: 19.5814058840008
Step 41:
Mobile Platform 1 Pose: [-8.51057154034963;-3.66871554942081;-7.22031568084674]
Robot Arm 1 end: [-3.34414003937429;-6.54835038535085;0.871734649038818]
Robot Arm 1 togoal: 21.2681809133441
Mobile Platform 2 Pose: [13.1329394216372;1.18235696663934;6.50088420090076]
Robot Arm 2 end: [0.507181520907665;6.41062441994792;0.252886160912359]
Robot Arm 1 togoal: 19.5273544054176
Step 42:
Mobile Platform 1 Pose: [-9.26124219739691;-3.42173241437626;-7.18159448505431]
Robot Arm 1 end: [-3.21391887196726;-6.39967484331443;0.663157438148121]
Robot Arm 1 togoal: 21.0782561134051
Mobile Platform 2 Pose: [13.615417810786;0.932651896909144;6.48874426057784]
Robot Arm 2 end: [0.233108006467865;6.31371714200076;0.426866725702215]
Robot Arm 1 togoal: 19.2886234367093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.565059e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-9.80570994642814;-3.21204708305503;-7.04546196935921]
Robot Arm 1 end: [-2.87073539997082;-6.33387401803792;0.892603756816415]
Robot Arm 1 togoal: 20.8049549870132
Mobile Platform 2 Pose: [14.9870202531711;0.188923333594321;6.4836542785074]
Robot Arm 2 end: [-0.476418666836012;6.49163289022203;0.272012936672995]
Robot Arm 1 togoal: 19.0848768788169
Step 44:
Mobile Platform 1 Pose: [-10.2664762456265;-3.02948358575775;-6.87466801101146]
Robot Arm 1 end: [-2.84440668269817;-6.12683121974531;1.00366211067394]
Robot Arm 1 togoal: 20.6233146393209
Mobile Platform 2 Pose: [15.5693502595367;-0.0609748519171332;6.47652007370884]
Robot Arm 2 end: [-0.666167518918706;6.28686369046813;0.195702074596004]
Robot Arm 1 togoal: 18.8185751917582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.735888e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-10.2506478561951;-3.02962587965465;-6.64353822502202]
Robot Arm 1 end: [-2.98856964965573;-5.91031945156591;1.10074308465813]
Robot Arm 1 togoal: 20.54307237172
Mobile Platform 2 Pose: [16.2914597087382;-0.558950132350699;6.491051750402]
Robot Arm 2 end: [-1.2419553896912;6.34483388755779;0.338947249689512]
Robot Arm 1 togoal: 18.5810168966215

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.608923e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-9.82252641644133;-3.23310996887632;-6.49829971279595]
Robot Arm 1 end: [-3.32797617952162;-5.82122483633123;1.13756321144254]
Robot Arm 1 togoal: 20.6904115022988
Mobile Platform 2 Pose: [18.1214780976465;-1.66941145288411;6.46737250905917]
Robot Arm 2 end: [-1.86022778943733;5.8794749261144;0.209272692716931]
Robot Arm 1 togoal: 17.8922956631511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.576557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-9.45087372395018;-3.40330698063975;-6.31517956826594]
Robot Arm 1 end: [-3.54216162381009;-5.81680289552801;1.29154824879568]
Robot Arm 1 togoal: 20.8234013404993
Mobile Platform 2 Pose: [18.3859763809328;-2.02543837613217;6.37197321832739]
Robot Arm 2 end: [-2.50466284254755;6.89204554261466;0.300324679719997]
Robot Arm 1 togoal: 18.5205650549651

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.693979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-9.64912284130232;-3.39024267570285;-6.06552115375363]
Robot Arm 1 end: [-3.74339150962307;-5.50970591346597;1.09971198166269]
Robot Arm 1 togoal: 20.7270179066689
Mobile Platform 2 Pose: [18.5626950787941;-2.12867199464185;6.37583117265944]
Robot Arm 2 end: [-2.70909774065764;6.13167394207466;0.214454176216416]
Robot Arm 1 togoal: 17.7509217202471

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.019846e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-9.51481033858166;-3.55508626261064;-5.87756745771147]
Robot Arm 1 end: [-3.77017571600879;-5.19303821146231;1.06110720539366]
Robot Arm 1 togoal: 20.5099402906796
Mobile Platform 2 Pose: [19.6664282454429;-2.74165575954874;6.31471399107691]
Robot Arm 2 end: [-2.69090586148527;6.68827403789149;1.28719661355264]
Robot Arm 1 togoal: 18.220208876765

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.360610e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.178573e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-9.51481033897502;-3.5550862587494;-5.52401407035291]
Robot Arm 1 end: [-3.60432511395949;-4.92023594942258;1.23750032431486]
Robot Arm 1 togoal: 20.1933522392609
Mobile Platform 2 Pose: [19.2482109837292;-2.49285321825316;6.29027436248252]
Robot Arm 2 end: [-2.24074568024383;6.74017008596983;1.19171410268016]
Robot Arm 1 togoal: 18.453935929986

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.472341e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-9.32676113060942;-3.63969947948511;-5.28876823536999]
Robot Arm 1 end: [-3.77305792197182;-4.79036314431302;1.29222635955671]
Robot Arm 1 togoal: 20.2114894018527
Mobile Platform 2 Pose: [19.0212254138749;-2.36932378230258;6.25970357581347]
Robot Arm 2 end: [-1.94328177502169;6.71446769101692;1.04426665324583]
Robot Arm 1 togoal: 18.5610186529364

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.244171e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-9.74762432253189;-3.51608277959416;-5.07146910592464]
Robot Arm 1 end: [-3.58855386950134;-4.41075262186612;1.16711300179353]
Robot Arm 1 togoal: 19.8101949359531
Mobile Platform 2 Pose: [19.5432611030554;-2.67380431104557;6.27655662241368]
Robot Arm 2 end: [-2.35847964454901;6.8418990422238;0.905454850462181]
Robot Arm 1 togoal: 18.5046289485185

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.417985e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-9.95603949011936;-3.47535247623609;-4.82480933518745]
Robot Arm 1 end: [-3.44229532310757;-4.2937910717875;1.32417829185997]
Robot Arm 1 togoal: 19.6226019055157
Mobile Platform 2 Pose: [19.5053639075596;-2.76025396408407;6.06855633441506]
Robot Arm 2 end: [-2.71505295278969;6.61395069840546;0.255005787716865]
Robot Arm 1 togoal: 18.1850639986567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.106904e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-9.74338269528427;-3.54793158186486;-4.58557665885303]
Robot Arm 1 end: [-3.57306826744892;-4.15610513848739;1.37114212576724]
Robot Arm 1 togoal: 19.6123928236003
Mobile Platform 2 Pose: [19.831722366032;-2.94996072972953;6.08133361692499]
Robot Arm 2 end: [-2.88933172884444;6.54169015273454;0.116641652826965]
Robot Arm 1 togoal: 18.0599373957599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.249412e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-10.3016183463095;-3.37042480787272;-4.40953306996976]
Robot Arm 1 end: [-3.48710801685428;-3.94009139301763;1.31799255034547]
Robot Arm 1 togoal: 19.3976659092942
Mobile Platform 2 Pose: [20.2632446622881;-3.19920716165954;6.10073800342102]
Robot Arm 2 end: [-3.18560457099549;6.46380788066374;0.0210805864313471]
Robot Arm 1 togoal: 17.8813509903878

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.951896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-10.9352618843731;-3.15411594153993;-4.28419163888715]
Robot Arm 1 end: [-3.45474241598339;-3.7173480629526;1.12908559826362]
Robot Arm 1 togoal: 19.218463212727
Mobile Platform 2 Pose: [20.7514441373808;-3.48291004690681;6.13173396633994]
Robot Arm 2 end: [-3.5246974316243;6.41890588465395;-0.0411897369430519]
Robot Arm 1 togoal: 17.718646973608

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.105469e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-11.3385637509027;-3.06238519526104;-4.05162880825274]
Robot Arm 1 end: [-3.4776112173563;-3.48602017432975;1.08808224758222]
Robot Arm 1 togoal: 19.0710818269178
Mobile Platform 2 Pose: [20.8930895144582;-3.57097441396172;6.11963702134386]
Robot Arm 2 end: [-3.81980481077646;6.38745956522844;0.0724338098253958]
Robot Arm 1 togoal: 17.5739007504372

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.867377e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-11.8165073715622;-2.8784895905351;-3.88226999814072]
Robot Arm 1 end: [-3.11296604847829;-3.3322047148692;1.24456689638718]
Robot Arm 1 togoal: 18.7022452237291
Mobile Platform 2 Pose: [20.8918626834333;-3.56739219071987;6.10225730472257]
Robot Arm 2 end: [-3.62152908806399;6.44338661573511;0.142814893853948]
Robot Arm 1 togoal: 17.6909369100608

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.306832e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-11.898684486217;-2.95200905620024;-3.64332463053997]
Robot Arm 1 end: [-3.09766956749872;-3.12796890836911;1.29893836842426]
Robot Arm 1 togoal: 18.5456686891227
Mobile Platform 2 Pose: [21.1097048100039;-3.70928223075691;5.85053994285971]
Robot Arm 2 end: [-3.19980730366223;5.49398362987868;0.605538092675272]
Robot Arm 1 togoal: 16.9453303282656

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.405707e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.235925e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-11.5901687873422;-3.19835323866568;-3.38232594976913]
Robot Arm 1 end: [-3.57189252387419;-2.82339721443482;1.11386167530908]
Robot Arm 1 togoal: 18.6762293949876
Mobile Platform 2 Pose: [20.9161325100503;-3.63842001060087;5.7981636829803]
Robot Arm 2 end: [-4.04080179491162;5.6800996214844;1.1266688313388]
Robot Arm 1 togoal: 16.7789470604618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.250071e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.040297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-11.1572988675443;-3.51586148697012;-3.23781066686916]
Robot Arm 1 end: [-3.74228519036167;-2.53431910721386;1.05791710902169]
Robot Arm 1 togoal: 18.6057518928587
Mobile Platform 2 Pose: [21.1110250239404;-3.66705514539177;5.675625453062]
Robot Arm 2 end: [-4.22864101895006;5.70794227263308;0.977460376862678]
Robot Arm 1 togoal: 16.7434605219602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.833719e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-10.7344282479228;-3.7516771080438;-3.15479866257078]
Robot Arm 1 end: [-4.02745995212239;-2.59210455776994;1.18285721675838]
Robot Arm 1 togoal: 18.853250925081
Mobile Platform 2 Pose: [21.81419109148;-3.96741082622609;5.54414519259178]
Robot Arm 2 end: [-4.59472612323704;5.75929581139466;0.796809460928162]
Robot Arm 1 togoal: 16.676235848978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.247572e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-10.378655400199;-3.97696270095605;-3.046417844959]
Robot Arm 1 end: [-4.42536015319577;-2.49353963517946;1.01594922069152]
Robot Arm 1 togoal: 19.090170885953
Mobile Platform 2 Pose: [22.8672198390319;-4.50505496812433;5.46916764497542]
Robot Arm 2 end: [-4.76568282634802;5.14967955617832;-0.0211731730457788]
Robot Arm 1 togoal: 16.1024583534607

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.679605e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-10.5241227400041;-4.00997111290433;-2.79860653621654]
Robot Arm 1 end: [-4.54500984049141;-2.30509345822888;0.926998203947084]
Robot Arm 1 togoal: 19.0610984575772
Mobile Platform 2 Pose: [23.1859604083671;-4.72106596484976;5.45663681720308]
Robot Arm 2 end: [-5.07228143869387;5.1669841482587;0.0520775441319599]
Robot Arm 1 togoal: 16.0149165328554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.465884e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-11.0860502023796;-3.80458528572258;-2.65607000758783]
Robot Arm 1 end: [-4.30934494224225;-2.03542198826914;0.722676898905873]
Robot Arm 1 togoal: 18.7148618719329
Mobile Platform 2 Pose: [23.7695413664795;-5.11627338452993;5.41036875807068]
Robot Arm 2 end: [-5.66194018596808;5.21357957862208;0.158795510436305]
Robot Arm 1 togoal: 15.8785190624495

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.762392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-11.2716161081432;-3.78505136400483;-2.50830277872262]
Robot Arm 1 end: [-4.14959656161078;-1.81309276713241;0.906683431284734]
Robot Arm 1 togoal: 18.4428205117282
Mobile Platform 2 Pose: [25.1551961476418;-5.87124381886648;5.37071553022719]
Robot Arm 2 end: [-5.19559759244373;5.44883565564977;0.297390423202589]
Robot Arm 1 togoal: 16.2248582738167

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-11.0786472347544;-3.9578475620254;-2.32763326286628]
Robot Arm 1 end: [-4.11816213961231;-1.64970149409883;1.08233149541617]
Robot Arm 1 togoal: 18.3093002913521
Mobile Platform 2 Pose: [24.921838021365;-5.726139461289;5.35053621070671]
Robot Arm 2 end: [-5.99016572277738;4.96396990178758;0.0469132071712509]
Robot Arm 1 togoal: 15.5618796430632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.324312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-10.6681748137434;-4.16854640238583;-2.19307473035746]
Robot Arm 1 end: [-4.52023086641814;-1.51243435310766;0.905000026957735]
Robot Arm 1 togoal: 18.5405691691351
Mobile Platform 2 Pose: [24.940871525557;-5.77250523108505;5.37017362472704]
Robot Arm 2 end: [-6.13642613691262;5.11567959590596;0.0780951122851301]
Robot Arm 1 togoal: 15.6682145490994

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.756773e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-10.4540560219137;-4.33984143502773;-1.9037569755086]
Robot Arm 1 end: [-4.5399911282596;-1.12146240481119;0.834150298621662]
Robot Arm 1 togoal: 18.318570646854
Mobile Platform 2 Pose: [25.3173454413189;-6.06595464694957;5.38833459560266]
Robot Arm 2 end: [-6.56205980491924;5.42961007687599;0.883054933711437]
Robot Arm 1 togoal: 15.820849339305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.155035e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.474492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-10.3114003521794;-4.45929458272004;-1.59832889774049]
Robot Arm 1 end: [-4.31719224723639;-2.05415523285037;1.29513686680884]
Robot Arm 1 togoal: 18.7172558452599
Mobile Platform 2 Pose: [25.0459985052346;-5.88049649413859;5.36226823947721]
Robot Arm 2 end: [-6.32285231110311;5.40520329368256;0.896301507259891]
Robot Arm 1 togoal: 15.8502981628889

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.008859e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.817624e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-10.2378868363873;-4.49814222441442;-1.57340830611753]
Robot Arm 1 end: [-4.48336423093492;-1.14160712710398;1.36980926887907]
Robot Arm 1 togoal: 18.2736451606634
Mobile Platform 2 Pose: [24.9079991577866;-5.78418117422285;5.34544446277078]
Robot Arm 2 end: [-5.9612094998785;5.49966576526846;0.829687775482808]
Robot Arm 1 togoal: 16.0321358567932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.625313e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.201357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-10.31538648124;-4.43614250853226;-1.2341394260237]
Robot Arm 1 end: [-4.27083463893382;-2.07199066819424;0.483324837451521]
Robot Arm 1 togoal: 18.7207491764548
Mobile Platform 2 Pose: [25.0035519264729;-5.82907840385489;5.276065845222]
Robot Arm 2 end: [-5.88256372010944;5.43987997267323;0.782084163196957]
Robot Arm 1 togoal: 15.9965362408052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.113126e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.459936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-10.3013774478631;-4.43580974294166;-1.23655465179782]
Robot Arm 1 end: [-4.03068495022291;-1.88857501515625;1.03414967655613]
Robot Arm 1 togoal: 18.3966127126938
Mobile Platform 2 Pose: [25.2734653462279;-5.98142633772871;5.24600766229593]
Robot Arm 2 end: [-6.17619247216689;4.95839794521503;0.600164547421846]
Robot Arm 1 togoal: 15.4668390768137

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.152977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.540963e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-10.0931205408974;-4.49229028753542;-1.20576154923485]
Robot Arm 1 end: [-4.59463046995122;-1.11809996818868;1.4808024595812]
Robot Arm 1 togoal: 18.3471251508055
Mobile Platform 2 Pose: [25.8714878499577;-6.41643010364058;5.21133690437749]
Robot Arm 2 end: [-6.76330666087429;4.91047184678987;1.05441207973182]
Robot Arm 1 togoal: 15.2648635353479

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.118126e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-10.3385293819197;-4.33365192319264;-1.18735547605416]
Robot Arm 1 end: [-3.53734783797679;-1.44618018399429;0.913336068228258]
Robot Arm 1 togoal: 17.7382096826262
Mobile Platform 2 Pose: [26.311565617579;-6.68265812333366;5.19943821860239]
Robot Arm 2 end: [-6.85359820571652;4.94530018428048;1.36421245936835]
Robot Arm 1 togoal: 15.2737167769414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.244328e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.685237e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-10.8135546813568;-4.08751379655656;-1.23112790384242]
Robot Arm 1 end: [-3.36503901819439;-1.57730514680899;0.990378012976615]
Robot Arm 1 togoal: 17.6901052035679
Mobile Platform 2 Pose: [27.3367013647806;-7.33545369730117;5.05921051824545]
Robot Arm 2 end: [-7.33153694057131;4.88640599576481;1.37348164400627]
Robot Arm 1 togoal: 15.124402144734

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.437643e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.591087e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-11.3218224128395;-3.78835540992166;-1.27592787115111]
Robot Arm 1 end: [-3.48209609897139;-1.26519793799905;1.24037617451396]
Robot Arm 1 togoal: 17.5712933370207
Mobile Platform 2 Pose: [27.2685232108613;-7.26774963705969;5.05484066451599]
Robot Arm 2 end: [-6.93433558795048;4.59164489893841;1.19049733067977]
Robot Arm 1 togoal: 14.9138737810956

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.230977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.899203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-12.2920597995569;-3.24075454198491;-1.30434351438514]
Robot Arm 1 end: [-2.86222750065809;-1.75330546426371;0.375589490039786]
Robot Arm 1 togoal: 17.4610724427597
Mobile Platform 2 Pose: [27.1406282922274;-7.16543370215259;4.73071313640594]
Robot Arm 2 end: [-7.22342915446949;4.78724741928461;0.703860921721942]
Robot Arm 1 togoal: 15.0678382050193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.965181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.261804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-12.0793822717639;-3.40127489321355;-1.2778905141875]
Robot Arm 1 end: [-2.96895267471236;-1.68495816217982;0.456056157765878]
Robot Arm 1 togoal: 17.48902759212
Mobile Platform 2 Pose: [27.6371942068714;-7.53963124068358;4.39370240747019]
Robot Arm 2 end: [-7.46920614468556;4.71599346869741;0.948451237376528]
Robot Arm 1 togoal: 14.9429981394104

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.579793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.158189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-12.210623171561;-3.31864297761269;-1.29402452076467]
Robot Arm 1 end: [-2.64538477610597;-1.47555870782816;0.310392735621639]
Robot Arm 1 togoal: 17.1189886892056
Mobile Platform 2 Pose: [27.1689798516892;-7.29258304073453;4.35475059916447]
Robot Arm 2 end: [-7.08397446089739;4.56960980467528;0.847946187484287]
Robot Arm 1 togoal: 14.8737936061965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.157553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.806413e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-13.1227845537665;-2.78265731064225;-1.30272111213084]
Robot Arm 1 end: [-2.10377108236148;-1.61771366083416;0.503971031898741]
Robot Arm 1 togoal: 16.8079413683324
Mobile Platform 2 Pose: [26.7740499452108;-7.0474835053804;4.29291789974993]
Robot Arm 2 end: [-6.71420767208533;4.38482846482487;0.84170141389686]
Robot Arm 1 togoal: 14.7709569004077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.473298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.061508e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-13.9141906937746;-2.27082269152304;-1.31828780587008]
Robot Arm 1 end: [-1.53176355574103;-1.41772220495796;0.356880275794561]
Robot Arm 1 togoal: 16.2696381578507
Mobile Platform 2 Pose: [26.0271689908348;-6.54455392391193;4.27704076330096]
Robot Arm 2 end: [-6.28671431862098;4.51934749886316;0.0815780590626959]
Robot Arm 1 togoal: 15.0556261201031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.597774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-14.8081662242335;-1.74545658258343;-1.33987568261735]
Robot Arm 1 end: [-1.01057110681921;-1.57599928211206;0.536927489866116]
Robot Arm 1 togoal: 16.0063997883029
Mobile Platform 2 Pose: [25.7249488831747;-6.32987812682986;4.29017363342236]
Robot Arm 2 end: [-5.60763603278779;3.83241577730829;0.718116917103626]
Robot Arm 1 togoal: 14.5352402354276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.406868e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.265622e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-15.7120890117909;-1.21111863447867;-1.32358376405228]
Robot Arm 1 end: [-0.444122658231028;-1.37573447080808;0.404882119038571]
Robot Arm 1 togoal: 15.4833055893988
Mobile Platform 2 Pose: [25.6012106049949;-6.27854583361606;4.25828628255927]
Robot Arm 2 end: [-5.64668613192381;3.68080728053438;0.795098914300482]
Robot Arm 1 togoal: 14.3750743256279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.354719e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.100157e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-16.6388810628455;-0.672206833674488;-1.31902057752333]
Robot Arm 1 end: [0.0520955958116626;-1.58670928665178;0.539721182353041]
Robot Arm 1 togoal: 15.302795242903
Mobile Platform 2 Pose: [25.4310444362273;-6.25412610694966;4.22420734511229]
Robot Arm 2 end: [-6.12685191253641;3.92359082700062;1.40233748551266]
Robot Arm 1 togoal: 14.4527415536812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.170856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.715091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-17.4242909681442;-0.161571460334538;-1.30070388170544]
Robot Arm 1 end: [0.605694901406978;-1.38066370592025;0.411410350765308]
Robot Arm 1 togoal: 14.7987723768693
Mobile Platform 2 Pose: [25.6487718576476;-6.5023195692706;4.19420715160132]
Robot Arm 2 end: [-6.47527928342402;4.0614537762297;1.47979449832157]
Robot Arm 1 togoal: 14.4965456686851

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.472228e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-18.3230439367931;0.371732523988525;-1.29644697979513]
Robot Arm 1 end: [1.08708167319259;-1.58730422001215;0.540320276269369]
Robot Arm 1 togoal: 14.6515345581269
Mobile Platform 2 Pose: [26.4978750158689;-7.13406918492494;4.13042805980349]
Robot Arm 2 end: [-7.44429955264325;3.70236255432685;0.498543473252342]
Robot Arm 1 togoal: 13.9761139697674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.328843e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.190352e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-19.0632174634051;0.873320619904844;-1.28077354324003]
Robot Arm 1 end: [1.63999784386382;-1.38415301418497;0.417378880472034]
Robot Arm 1 togoal: 14.1670941084065
Mobile Platform 2 Pose: [27.1164373930895;-7.35824652166692;4.01808446390233]
Robot Arm 2 end: [-7.85690330293447;3.71525652238913;0.526109116828457]
Robot Arm 1 togoal: 13.917288988784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.720661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.878939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-19.9341180573347;1.40105413164466;-1.27815883609276]
Robot Arm 1 end: [2.11382917579875;-1.5924805468844;0.53813702403095]
Robot Arm 1 togoal: 14.0550103211598
Mobile Platform 2 Pose: [27.7146122459035;-7.67297162637535;3.94253855758976]
Robot Arm 2 end: [-7.60294229596181;3.95703852015291;1.27186705025939]
Robot Arm 1 togoal: 14.1635757178817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.389745e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.749793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-20.6966768379212;1.92569326433533;-1.25299270750108]
Robot Arm 1 end: [2.66371805912136;-1.18214936084771;0.435416244775856]
Robot Arm 1 togoal: 13.4178980892678
Mobile Platform 2 Pose: [27.3515207466982;-7.43470280305879;3.86642569972351]
Robot Arm 2 end: [-7.31455167359791;4.03995127884478;1.22674500536493]
Robot Arm 1 togoal: 14.2974980548339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.551610e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.317095e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-21.6263336820815;2.46517802376105;-1.24846956456785]
Robot Arm 1 end: [3.14753482848807;-1.39546967710058;0.271399887651969]
Robot Arm 1 togoal: 13.3556919899076
Mobile Platform 2 Pose: [26.9345797209943;-7.21517762152952;3.74657183717728]
Robot Arm 2 end: [-7.01094361478167;4.03500915237252;1.12215197014946]
Robot Arm 1 togoal: 14.3553132927188

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.343865e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.792613e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-21.9855116988797;2.71316137809698;-1.21677925546249]
Robot Arm 1 end: [3.33528802666255;-1.4546245451371;0.25654814173421]
Robot Arm 1 togoal: 13.3126123529335
Mobile Platform 2 Pose: [26.6368873404415;-7.11250080572522;3.56283066415274]
Robot Arm 2 end: [-6.86167011265667;3.89179235680397;1.00220857593661]
Robot Arm 1 togoal: 14.2513211285141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.965907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.545427e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-22.2513647401372;2.95834968815586;-1.16796642302609]
Robot Arm 1 end: [3.53017117224356;-0.688681977358109;0.605665088936298]
Robot Arm 1 togoal: 12.5277470230277
Mobile Platform 2 Pose: [26.3244422102422;-7.00274359975664;3.40200567469927]
Robot Arm 2 end: [-6.89138810999462;3.68233613132042;0.763097801465977]
Robot Arm 1 togoal: 14.051484107939

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.160539e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.482134e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-22.9176515346489;3.33686683329154;-1.15268400057628]
Robot Arm 1 end: [4.00968985016484;-0.852754296577481;0.421592482901258]
Robot Arm 1 togoal: 12.4448699632319
Mobile Platform 2 Pose: [25.6400928233681;-6.64879707980472;3.1443330022111]
Robot Arm 2 end: [-6.18821483460138;3.78424688221175;0.783687850909086]
Robot Arm 1 togoal: 14.32057252564

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.101333e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.477386e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-23.950595909019;3.89700909875884;-1.11810155381175]
Robot Arm 1 end: [4.57792808926383;-1.03843240554773;0.256908721265494]
Robot Arm 1 togoal: 12.3630004666348
Mobile Platform 2 Pose: [25.7436207003709;-6.76014704511045;3.08224180243777]
Robot Arm 2 end: [-6.1385576895632;3.59119347192412;0.627389223910368]
Robot Arm 1 togoal: 14.1573237911424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.977288e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.643039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-24.7591284222532;4.45423253206003;-1.0208309754267]
Robot Arm 1 end: [3.76167718162543;-0.984858490316654;0.821738285150427]
Robot Arm 1 togoal: 12.6519861912819
Mobile Platform 2 Pose: [25.4727606722357;-6.68699187138192;2.99612845510996]
Robot Arm 2 end: [-5.96962382910258;3.29678680837621;0.473635090520309]
Robot Arm 1 togoal: 13.9336084615563

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.283026e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.130400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-24.1858216800733;4.09346171731597;-1.07608546388209]
Robot Arm 1 end: [3.35505365049478;-0.820088306615799;1.00938511148314]
Robot Arm 1 togoal: 12.7079255877944
Mobile Platform 2 Pose: [25.3384245875537;-6.67302183951531;2.94887150169945]
Robot Arm 2 end: [-6.28130121363461;2.74778446177847;0.00452508869179008]
Robot Arm 1 togoal: 13.3654190171615

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.376771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.242117e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-24.6685591711201;4.33762142334422;-1.0223636063561]
Robot Arm 1 end: [3.64386498812941;-0.568865161282204;0.791451425470621]
Robot Arm 1 togoal: 12.3544973272525
Mobile Platform 2 Pose: [25.2253470881012;-6.63366985974826;2.90123473148253]
Robot Arm 2 end: [-7.07403082151082;2.58993000707144;-0.0235556900776361]
Robot Arm 1 togoal: 13.0174223829519

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.842645e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.342946e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-24.8304928920548;4.42060813505262;-1.00611832065141]
Robot Arm 1 end: [3.91213918346991;-0.475445780541914;0.378100509684899]
Robot Arm 1 togoal: 12.1697671657053
Mobile Platform 2 Pose: [24.323258588007;-6.1552918411619;2.98416157204754]
Robot Arm 2 end: [-6.42225941832688;2.75421619155062;-0.152308003008666]
Robot Arm 1 togoal: 13.3517870715307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.176925e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.730974e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-24.6957092039338;4.34231208858825;-1.01160576954556]
Robot Arm 1 end: [3.65987946171534;-0.710730187842648;0.435529492655241]
Robot Arm 1 togoal: 12.4938030966979
Mobile Platform 2 Pose: [23.7940491801953;-5.86603994739947;3.09534621867249]
Robot Arm 2 end: [-5.855596627659;3.31733861021542;-0.155410041171779]
Robot Arm 1 togoal: 14.0477022177275

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.455443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.192611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-23.8950070790817;3.82861827302456;-1.01543898732112]
Robot Arm 1 end: [3.15981675512883;-0.552647739483954;0.677759328810384]
Robot Arm 1 togoal: 12.6038698805803
Mobile Platform 2 Pose: [23.7418146788083;-5.8312391768373;3.10914680505112]
Robot Arm 2 end: [-5.30729724633587;3.18057675130217;-0.0268907490632245]
Robot Arm 1 togoal: 14.0763996960249

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.050055e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-23.7462803894097;3.74096489115573;-1.02134356548444]
Robot Arm 1 end: [3.30874389610755;-0.569837350244561;1.15025701587338]
Robot Arm 1 togoal: 12.5152634897865
Mobile Platform 2 Pose: [23.6108640585669;-5.73249370209561;3.1091598560565]
Robot Arm 2 end: [-5.1594656150434;3.22168893993531;0.136406651981317]
Robot Arm 1 togoal: 14.1478242247963

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.130416e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.396621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-23.7869547495757;3.76859656817989;-1.02434233945066]
Robot Arm 1 end: [3.44531018271581;-1.07675517869626;1.32934620319668]
Robot Arm 1 togoal: 12.8722645664006
Mobile Platform 2 Pose: [23.8055302368726;-5.86788478599452;3.10985494088024]
Robot Arm 2 end: [-5.46183869988303;2.93441476083485;0.0533128517388923]
Robot Arm 1 togoal: 13.785793366954

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006525e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.079403e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-23.7623937873362;3.74069338399654;-1.01409602034269]
Robot Arm 1 end: [3.64221885015743;-1.20053707921238;1.33004134578354]
Robot Arm 1 togoal: 12.8806008097342
Mobile Platform 2 Pose: [24.1591637297998;-6.08406769789534;3.13336575360456]
Robot Arm 2 end: [-6.25163741135975;3.26050929958283;-0.0264836663772839]
Robot Arm 1 togoal: 13.8667240066984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.339468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.607029e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-23.7873907294504;3.76077572376014;-1.01057667664667]
Robot Arm 1 end: [3.48737165763533;-1.00271307318357;1.38984273708438]
Robot Arm 1 togoal: 12.7863687231439
Mobile Platform 2 Pose: [23.7169821517708;-5.73032243547213;2.84731104323244]
Robot Arm 2 end: [-5.83668586695017;2.89865313433208;0.789445639898588]
Robot Arm 1 togoal: 13.5736365441267

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.916651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.152372e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-23.6302544328882;3.65920455350865;-1.03035926909509]
Robot Arm 1 end: [3.38200340568013;-1.12724099043042;1.38070182148515]
Robot Arm 1 togoal: 12.9473184312598
Mobile Platform 2 Pose: [23.5239169866159;-5.58724171336473;2.77525049309895]
Robot Arm 2 end: [-5.74539901670336;2.87375704632408;0.822682131756587]
Robot Arm 1 togoal: 13.5765569221344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.781357e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.993819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-23.6294369002902;3.6364815741234;-0.999812046596601]
Robot Arm 1 end: [3.16407658636033;-1.32706088245814;0.734586524202439]
Robot Arm 1 togoal: 13.2533242360917
Mobile Platform 2 Pose: [23.3156704377397;-5.43608769556761;2.662104087731]
Robot Arm 2 end: [-5.52048739104113;2.83530812581405;0.9140415546683]
Robot Arm 1 togoal: 13.6080699018182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.896724e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.344716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-24.0521618014922;3.83556272164217;-0.873800318157226]
Robot Arm 1 end: [3.39352751951789;-1.13024763585075;0.618576688601124]
Robot Arm 1 togoal: 12.974677603073
Mobile Platform 2 Pose: [23.3106135763363;-5.43116833020982;2.53188104665599]
Robot Arm 2 end: [-5.48844555500827;2.65963969241431;1.01922935249094]
Robot Arm 1 togoal: 13.4488800438764

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.282280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-24.4536175268763;4.05236471710054;-0.758475906292252]
Robot Arm 1 end: [3.68028784341017;-0.952546532886177;0.537835062799422]
Robot Arm 1 togoal: 12.6832034829899
Mobile Platform 2 Pose: [23.5618196543261;-5.59177595261664;2.43434237860583]
Robot Arm 2 end: [-5.83450189203832;2.36018175889442;1.31985143834858]
Robot Arm 1 togoal: 13.0447663200302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.670208e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.693488e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-24.7795011030716;4.25640036233947;-0.67188065031134]
Robot Arm 1 end: [4.35370853257341;-0.726668621468023;0.628214041005296]
Robot Arm 1 togoal: 12.1547971525032
Mobile Platform 2 Pose: [23.8165937195901;-5.75378617520856;2.41664993549297]
Robot Arm 2 end: [-5.4874285833035;1.83739694248466;1.22933069615094]
Robot Arm 1 togoal: 12.6717140966619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.288894e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.023291e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-25.1520936077282;4.52908722819428;-0.599155858938752]
Robot Arm 1 end: [5.10715352499533;-0.820615868202719;0.55849840133919]
Robot Arm 1 togoal: 11.9143645958394
Mobile Platform 2 Pose: [23.8110402518375;-5.75279197841876;2.41079725754306]
Robot Arm 2 end: [-5.88950260258415;2.43699794874049;1.49558059917205]
Robot Arm 1 togoal: 13.0986927964998

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.766342e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.124194e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-25.6401606146033;4.88458112562813;-0.570551687801623]
Robot Arm 1 end: [5.20032687667852;-0.584253449617795;-0.0751972665849325]
Robot Arm 1 togoal: 11.7307769943143
Mobile Platform 2 Pose: [24.4996867018745;-6.19491192886371;2.37404531819338]
Robot Arm 2 end: [-6.29370160055211;2.23448545586546;1.48663774996116]
Robot Arm 1 togoal: 12.7836013298194

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.927810e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.250523e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-25.7814991174992;4.97300918510414;-0.576175081579637]
Robot Arm 1 end: [5.27404342607434;-0.632732120006652;0.0222488030354472]
Robot Arm 1 togoal: 11.7318333190024
Mobile Platform 2 Pose: [24.7902722026955;-6.40715541938972;2.38102548900173]
Robot Arm 2 end: [-6.76455237519578;2.81707426066254;1.27505364162277]
Robot Arm 1 togoal: 13.2214221454195

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.995653e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007458e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-25.8201304872354;4.98957300747598;-0.593332555719811]
Robot Arm 1 end: [5.58562457201417;-0.58396302756202;0.132557849802026]
Robot Arm 1 togoal: 11.5513962529406
Mobile Platform 2 Pose: [24.7228871981277;-6.36794780757839;2.38311595340094]
Robot Arm 2 end: [-6.72749126967714;2.96767699365949;1.10110513298114]
Robot Arm 1 togoal: 13.3808173031872

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.405226e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966699e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-25.5433674989239;4.86144791531306;-0.53175843457353]
Robot Arm 1 end: [5.35420899284281;-0.0070347865566307;0.744752520213178]
Robot Arm 1 togoal: 11.0601392148296
Mobile Platform 2 Pose: [24.4718507534369;-6.22269224260206;2.37581981252766]
Robot Arm 2 end: [-6.40771837255326;3.10788816736927;0.793340563905381]
Robot Arm 1 togoal: 13.6106835887875

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.530644e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.331503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-25.9805569381432;5.11742440103008;-0.492179143532811]
Robot Arm 1 end: [5.76314745930909;-0.70602911662423;0.318143512346096]
Robot Arm 1 togoal: 11.576564370635
Mobile Platform 2 Pose: [24.2968062601637;-6.12194721058817;2.37998012093326]
Robot Arm 2 end: [-6.2676911964413;3.08800722655298;0.685317042965127]
Robot Arm 1 togoal: 13.6354108178706

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.657573e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-25.9359303674501;5.15149676758888;-0.436821194644784]
Robot Arm 1 end: [5.65984347264261;0.126843647835334;0.772994653598051]
Robot Arm 1 togoal: 10.8109059303325
Mobile Platform 2 Pose: [24.3677021239786;-6.16319135285232;2.37603644932976]
Robot Arm 2 end: [-6.4096529092088;3.03407165013972;0.515715141605]
Robot Arm 1 togoal: 13.556851170528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.923430e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.220542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-26.4253333580538;5.42810122601658;-0.399491072869623]
Robot Arm 1 end: [6.11803070928242;-0.777950440870314;0.618580321879471]
Robot Arm 1 togoal: 11.4912254592533
Mobile Platform 2 Pose: [23.8939046858371;-5.89649634948445;2.42712419165514]
Robot Arm 2 end: [-5.58276253679545;2.89967049939641;0.358291977130688]
Robot Arm 1 togoal: 13.6844939893947

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.338775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-26.4582473035017;5.52973083390867;-0.35254895842292]
Robot Arm 1 end: [5.93461074656898;0.274095828755852;0.361610991428906]
Robot Arm 1 togoal: 10.6049415181477
Mobile Platform 2 Pose: [23.2290915826;-5.55808591420581;2.35770627637136]
Robot Arm 2 end: [-5.31750623845462;2.26526091807209;0.222720868499086]
Robot Arm 1 togoal: 13.1927215508624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.416645e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.370102e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-26.829969760469;5.74422946299249;-0.327384891981166]
Robot Arm 1 end: [6.5337384203719;-0.434887698359167;0.541482693933348]
Robot Arm 1 togoal: 11.0390807936466
Mobile Platform 2 Pose: [22.8663347617095;-5.3481174952476;2.22200942781692]
Robot Arm 2 end: [-5.22022035414667;2.04508875143522;0.241173055234241]
Robot Arm 1 togoal: 13.0218437059202

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.634962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.673201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-27.1014222922524;6.04839199050202;-0.199219802926725]
Robot Arm 1 end: [6.67476027714117;0.228299147189599;0.744116087771032]
Robot Arm 1 togoal: 10.3511789369315
Mobile Platform 2 Pose: [23.0409834084889;-5.56598226578722;2.04201356024363]
Robot Arm 2 end: [-5.63232046233255;1.79823982356931;0.215695947443569]
Robot Arm 1 togoal: 12.6482768054718

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.388371e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.663399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-27.573764576397;6.32163213854947;-0.170587387548781]
Robot Arm 1 end: [6.88070607806862;-0.368823873973695;0.130413984242609]
Robot Arm 1 togoal: 10.9167867327389
Mobile Platform 2 Pose: [23.6827346487032;-6.02308741354321;1.88258033129658]
Robot Arm 2 end: [-6.31093052022197;1.7261726750251;-0.0318508088592659]
Robot Arm 1 togoal: 12.3904683069572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.312704e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.786746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-27.7322287062523;6.4532263581958;-0.142887582940223]
Robot Arm 1 end: [7.2202730850042;-0.0490537167978528;0.776983723052165]
Robot Arm 1 togoal: 10.452938464633
Mobile Platform 2 Pose: [23.6452784653173;-6.00347885751333;1.89163333298546]
Robot Arm 2 end: [-6.11646300848905;1.93118947529937;-0.0409433672355257]
Robot Arm 1 togoal: 12.6441610612322

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.856046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.876157e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-28.6476455117063;6.98986310987987;-0.0907540708684972]
Robot Arm 1 end: [7.79986893153368;-0.12367713167493;0.570930696631988]
Robot Arm 1 togoal: 10.4034632246205
Mobile Platform 2 Pose: [23.3740869416192;-5.82383189611522;1.8781995150297]
Robot Arm 2 end: [-5.78462473794958;2.04753577349402;-0.00795025535277699]
Robot Arm 1 togoal: 12.8549677439062

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.909791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.471875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-29.2129171951316;7.39307069257051;0.00464575724588925]
Robot Arm 1 end: [7.75549204533849;0.24159126659323;1.27295287586658]
Robot Arm 1 togoal: 10.0162809636391
Mobile Platform 2 Pose: [22.508823714077;-5.2972258600135;1.91791231057744]
Robot Arm 2 end: [-4.54568693863127;2.12731972114854;0.433577161239438]
Robot Arm 1 togoal: 13.3418103352069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.512994e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.973004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-29.6319079292498;7.64297324331312;0.0116533627132009]
Robot Arm 1 end: [8.1139096476914;0.275329300776536;1.13546982412614]
Robot Arm 1 togoal: 9.91338441410986
Mobile Platform 2 Pose: [22.4729318764128;-5.24331331980606;1.93264417429829]
Robot Arm 2 end: [-4.53252696972573;2.0602497659316;0.372153284093806]
Robot Arm 1 togoal: 13.2914534393376

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.154840e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.835249e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-29.8831329306678;7.79631966569168;0.0262214752232674]
Robot Arm 1 end: [8.56006388960581;-0.0236778918801396;0.709870296236331]
Robot Arm 1 togoal: 10.1590090992435
Mobile Platform 2 Pose: [22.4983197407457;-5.22110264228972;1.95307140129201]
Robot Arm 2 end: [-4.58026532902821;2.04115655031131;0.298199655250464]
Robot Arm 1 togoal: 13.26115438625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.255381e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.207917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-30.0304138995234;7.94461481101151;0.0979443493960633]
Robot Arm 1 end: [8.27148392118426;0.440638916754772;-0.0011243848579125]
Robot Arm 1 togoal: 9.8329046978788
Mobile Platform 2 Pose: [22.5692694167654;-5.23757974620737;1.97349838353831]
Robot Arm 2 end: [-4.61073417539419;2.28189920288412;0.499493818597726]
Robot Arm 1 togoal: 13.451122965638

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.017651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.863273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-30.2135458640774;8.05417833159983;0.105126703015583]
Robot Arm 1 end: [8.79842583933793;0.209966107320748;0.338591444184809]
Robot Arm 1 togoal: 9.93411465991027
Mobile Platform 2 Pose: [22.7886879415957;-5.38500623563854;1.98737675813208]
Robot Arm 2 end: [-5.19524446381872;2.3359987086433;0.153089735188918]
Robot Arm 1 togoal: 13.3091591918757

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.445226e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.412354e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-30.5190000046514;8.24432503335213;0.119733364248704]
Robot Arm 1 end: [9.01313941414946;-0.0994270037581638;0.703740220156781]
Robot Arm 1 togoal: 10.1803847259273
Mobile Platform 2 Pose: [22.3083028535539;-5.16151091568819;1.86632475329491]
Robot Arm 2 end: [-5.17176610564027;2.22750128787353;0.178177799507494]
Robot Arm 1 togoal: 13.2146434588955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.747979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.062576e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-30.5701288342627;8.3257890405463;0.164483874816283]
Robot Arm 1 end: [9.00969044250057;0.485402538485318;0.830682460121214]
Robot Arm 1 togoal: 9.59087150264253
Mobile Platform 2 Pose: [21.5452933032294;-4.73512705778182;1.78083665596965]
Robot Arm 2 end: [-4.91145888287263;2.11709219974261;0.250243161848405]
Robot Arm 1 togoal: 13.2034842757307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.092287e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.229061e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-31.022374270335;8.59281600054589;0.192590333922086]
Robot Arm 1 end: [9.31928227299661;0.106724564412656;0.325061213239978]
Robot Arm 1 togoal: 9.98852066373843
Mobile Platform 2 Pose: [20.7422669023355;-4.26186697698467;1.7407422852455]
Robot Arm 2 end: [-4.38638265104701;2.11708588514579;0.312187378447096]
Robot Arm 1 togoal: 13.4088663966232

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.376869e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.698637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-31.1096233180513;8.69846714085911;0.255306184033918]
Robot Arm 1 end: [9.34663135330686;0.538825513976429;0.919827018526791]
Robot Arm 1 togoal: 9.50274287756532
Mobile Platform 2 Pose: [19.5827450667224;-3.56268334359001;1.6213195696574]
Robot Arm 2 end: [-3.42332289777167;1.77826248962048;0.197376400583798]
Robot Arm 1 togoal: 13.5547824923274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.096048e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.452599e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-31.4543745612716;8.90550468776841;0.270792012154823]
Robot Arm 1 end: [9.65188195048643;0.180674016749268;0.458361266445701]
Robot Arm 1 togoal: 9.88279059880305
Mobile Platform 2 Pose: [20.1591484313492;-3.92111336104665;1.66349754342688]
Robot Arm 2 end: [-3.87921210031035;1.68316178763251;0.158290966069579]
Robot Arm 1 togoal: 13.2596112228126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.541736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.716195e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-32.3251473572919;9.5346044885409;0.401670561901384]
Robot Arm 1 end: [9.84823489690563;0.910765167891418;0.12272612024081]
Robot Arm 1 togoal: 9.1974122622915
Mobile Platform 2 Pose: [20.7654996419387;-4.3358177707572;1.62799399133701]
Robot Arm 2 end: [-4.22871203389144;1.4955007737036;0.116892535727963]
Robot Arm 1 togoal: 12.9393129878365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.518660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.675172e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-32.2734914987743;9.50452856633953;0.403735201699389]
Robot Arm 1 end: [10.0495719982885;0.836854757318634;0.174176448729141]
Robot Arm 1 togoal: 9.26172887746917
Mobile Platform 2 Pose: [20.9624466095629;-4.48562198869624;1.63491955542413]
Robot Arm 2 end: [-4.41101147980863;1.5725845189938;0.134955569633087]
Robot Arm 1 togoal: 12.9260444178038

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.480005e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.048421e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-32.2756490946347;9.51269850186848;0.438635022335412]
Robot Arm 1 end: [10.2591704837923;0.693765830054461;0.656341235936946]
Robot Arm 1 togoal: 9.34990901259483
Mobile Platform 2 Pose: [21.3030132574916;-4.73349639249988;1.6023883351232]
Robot Arm 2 end: [-4.71135356311611;1.6177565356578;0.156909637130627]
Robot Arm 1 togoal: 12.8375539170925

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.253002e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.679362e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-32.7754932204179;9.8111037202096;0.493177448643281]
Robot Arm 1 end: [10.4613589473127;0.0774708486430579;0.503977628882965]
Robot Arm 1 togoal: 9.98517758185333
Mobile Platform 2 Pose: [21.6244062890857;-4.95736909465585;1.49094091220222]
Robot Arm 2 end: [-4.92861512774043;1.40716093045487;0.187350732006337]
Robot Arm 1 togoal: 12.5547156632836

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.799566e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.480348e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-32.7112157564396;9.80255837572276;0.516034349064367]
Robot Arm 1 end: [10.522712951727;0.540248952053357;0.352306018994707]
Robot Arm 1 togoal: 9.54599205636945
Mobile Platform 2 Pose: [21.9477045804292;-5.19046894490332;1.40058350899114]
Robot Arm 2 end: [-5.30321186115802;0.943917654630565;0.745579982990057]
Robot Arm 1 togoal: 11.9344220075914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.728642e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.575475e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-33.4403003845489;10.3094283906567;0.663114245209666]
Robot Arm 1 end: [10.8480399055613;0.845881859653543;1.12177132355568]
Robot Arm 1 togoal: 9.20197990352837
Mobile Platform 2 Pose: [22.3004768326281;-5.37985137609699;1.29083622417644]
Robot Arm 2 end: [-5.33002859029579;0.7898736017597;0.917821017826722]
Robot Arm 1 togoal: 11.7725880839321

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.045196e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.124530e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-33.407209795858;10.2954759719819;0.678712530398902]
Robot Arm 1 end: [10.9217704494031;0.742603460306243;1.06008942722007]
Robot Arm 1 togoal: 9.31458480076418
Mobile Platform 2 Pose: [22.3986052733629;-5.60362280411633;1.17913499234436]
Robot Arm 2 end: [-5.77176439244234;0.647328982121099;0.888199908866588]
Robot Arm 1 togoal: 11.4736230878425

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.149227e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.786481e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-33.5132304200575;10.3762400191091;0.710263563623527]
Robot Arm 1 end: [11.1313230192589;0.795277284057077;0.441014794756599]
Robot Arm 1 togoal: 9.33665786536304
Mobile Platform 2 Pose: [22.780903024982;-5.94616343102846;1.09842537146778]
Robot Arm 2 end: [-6.312976044451;0.666722367456809;1.02649392740007]
Robot Arm 1 togoal: 11.2967981290733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.827448e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.657516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-34.1767303993315;10.8135970417486;0.797150053779874]
Robot Arm 1 end: [11.5946257881761;0.761361899235495;0.527494200617369]
Robot Arm 1 togoal: 9.42774199554806
Mobile Platform 2 Pose: [23.6001133804605;-6.37645672104541;0.990635087518046]
Robot Arm 2 end: [-6.323160577768;0.945693594208402;-0.0507515534374682]
Robot Arm 1 togoal: 11.6532295669873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.786638e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.983092e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-34.6192417579808;11.1125134067003;0.858928386085855]
Robot Arm 1 end: [11.8679324343254;0.653803946471715;0.508169207990489]
Robot Arm 1 togoal: 9.58469501149802
Mobile Platform 2 Pose: [23.3362382902581;-6.19414923722653;0.935104172395203]
Robot Arm 2 end: [-6.01610417334682;1.00343838964868;0.0209315499423962]
Robot Arm 1 togoal: 11.798190018273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.413242e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.212101e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-34.7274340843703;11.2034932310082;0.88372863244592]
Robot Arm 1 end: [11.9812651013928;0.955521103397963;0.513857969871403]
Robot Arm 1 togoal: 9.3135570531311
Mobile Platform 2 Pose: [22.6956146325689;-5.84500396587215;0.776340836923832]
Robot Arm 2 end: [-5.59013537646131;1.01673939693866;0.125709063974168]
Robot Arm 1 togoal: 11.9483174436468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.560023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.432669e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-35.1650443224855;11.4875196258662;0.943808602803066]
Robot Arm 1 end: [12.2320652568679;0.70280129189541;0.539800706374132]
Robot Arm 1 togoal: 9.6115956626728
Mobile Platform 2 Pose: [21.7135388282126;-5.29503541440763;0.751505593018604]
Robot Arm 2 end: [-5.18829737393296;1.20695525281066;0.234263093656754]
Robot Arm 1 togoal: 12.2639316724428

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.670158e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.221857e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-35.2236977110083;11.5425836094617;0.961943690003995]
Robot Arm 1 end: [12.3065260695185;0.986581604238239;0.454894819112432]
Robot Arm 1 togoal: 9.36473992928475
Mobile Platform 2 Pose: [21.2470957368891;-5.05188565250672;0.693128739929638]
Robot Arm 2 end: [-5.04600725903866;1.08638621945311;0.244705062406429]
Robot Arm 1 togoal: 12.2097883786782

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.215485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.860304e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-35.6306462340143;11.8243440708426;1.03922447181378]
Robot Arm 1 end: [12.6041915996449;1.1602465873254;0.614022879784546]
Robot Arm 1 togoal: 9.259895343963
Mobile Platform 2 Pose: [20.7470082009661;-4.8184166644483;0.603626284320501]
Robot Arm 2 end: [-4.90282187432651;0.955867454994454;0.288029640980823]
Robot Arm 1 togoal: 12.1462945972206

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.880803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.295813e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-36.0287818862687;12.0885752147457;1.10819408051865]
Robot Arm 1 end: [12.8525030036191;1.08922158738201;0.428960692330437]
Robot Arm 1 togoal: 9.41972903824363
Mobile Platform 2 Pose: [19.8017668182223;-4.27581499730628;0.533645308550344]
Robot Arm 2 end: [-4.69025512962762;0.772071828265089;0.594646294972012]
Robot Arm 1 togoal: 12.045291746096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.136885e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.722376e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-36.2638780956964;12.257570755965;1.1509514415564]
Robot Arm 1 end: [13.0369878072495;1.25774313828434;0.632749583586006]
Robot Arm 1 togoal: 9.2972758795174
Mobile Platform 2 Pose: [19.8449100169334;-4.29936501624482;0.521855799880443]
Robot Arm 2 end: [-4.61443208980797;0.765137941496546;0.566031663990459]
Robot Arm 1 togoal: 12.0749534631617

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001641e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.131406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-36.5208367737275;12.4387627096931;1.20768636872833]
Robot Arm 1 end: [13.1969859836993;1.31129816784666;0.428454660874224]
Robot Arm 1 togoal: 9.32244143055241
Mobile Platform 2 Pose: [19.9659628744328;-4.37378228797593;0.515833762290544]
Robot Arm 2 end: [-4.57700705522977;0.732521611295108;0.472556341712245]
Robot Arm 1 togoal: 12.0704228062048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.243485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.565569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-36.6823245523065;12.5478081868802;1.23460729882866]
Robot Arm 1 end: [13.3324846041949;1.24990387870196;0.595190683177107]
Robot Arm 1 togoal: 9.40886596037307
Mobile Platform 2 Pose: [20.0184253335982;-4.42316296930447;0.534298389494643]
Robot Arm 2 end: [-4.42387181054673;0.549168442398111;0.594995420397591]
Robot Arm 1 togoal: 11.9681094953526

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.717206e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.708385e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-36.6745133207862;12.5565133668987;1.2556902328099]
Robot Arm 1 end: [13.3411756743092;1.26742806128558;0.589901413668888]
Robot Arm 1 togoal: 9.39618072031854
Mobile Platform 2 Pose: [19.968153869943;-4.392498455693;0.561954138193654]
Robot Arm 2 end: [-4.42907529417791;0.54752558604768;0.617775872405953]
Robot Arm 1 togoal: 11.9624960560469

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.409450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-36.8680377355803;12.6937316944419;1.29128544591329]
Robot Arm 1 end: [13.470342683614;1.33823371678338;0.549101761663269]
Robot Arm 1 togoal: 9.38158087277744
Mobile Platform 2 Pose: [20.125422150326;-4.50226246290338;0.59368994669756]
Robot Arm 2 end: [-4.74579332330967;0.294504453192843;0.435919300708962]
Robot Arm 1 togoal: 11.6086566777624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.762110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.237939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-37.1763526098357;12.8953904309116;1.32486879321389]
Robot Arm 1 end: [13.644250835124;1.24559588003686;0.457093348400246]
Robot Arm 1 togoal: 9.54211994308643
Mobile Platform 2 Pose: [20.5283442290796;-4.80932232326837;0.568154125174024]
Robot Arm 2 end: [-5.12391131022565;0.173973068463641;0.530763618732321]
Robot Arm 1 togoal: 11.3254817557204

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.689212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.050522e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-37.169639788364;12.915587812964;1.3583041856647]
Robot Arm 1 end: [13.6511683979072;1.50951078777361;0.431019895809514]
Robot Arm 1 togoal: 9.30631475721406
Mobile Platform 2 Pose: [20.9596219752523;-5.1220512717019;0.519190435967912]
Robot Arm 2 end: [-5.42828307958284;0.0525481788599447;0.662509686003655]
Robot Arm 1 togoal: 11.0766116709016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.064793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-36.9969144181918;12.8358498220053;1.40397991742903]
Robot Arm 1 end: [13.5416124805851;1.73952494282315;0.660778698121589]
Robot Arm 1 togoal: 9.02875671513427
Mobile Platform 2 Pose: [21.4262379054138;-5.44728297842481;0.442098189874499]
Robot Arm 2 end: [-6.01925005539863;-0.00175218718810044;0.933517838904835]
Robot Arm 1 togoal: 10.7775908596238

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.989758e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.752937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-36.6727972871576;12.6595613684793;1.43770112727847]
Robot Arm 1 end: [13.2705815375802;1.88383024142077;0.86875779991191]
Robot Arm 1 togoal: 8.77464158992334
Mobile Platform 2 Pose: [22.5226661430585;-6.14155921045654;0.379067498047355]
Robot Arm 2 end: [-6.21248577313564;-0.301479524987741;0.978871730943249]
Robot Arm 1 togoal: 10.4259515864521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.457384e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.134762e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-36.3629147070839;12.4837599499823;1.44835596654488]
Robot Arm 1 end: [12.8080634139677;1.89917057384489;1.2276475187889]
Robot Arm 1 togoal: 8.57873610777002
Mobile Platform 2 Pose: [22.5778277444647;-6.20978259347912;0.32379015901733]
Robot Arm 2 end: [-6.18508131792268;0.322088609842827;1.37217349981586]
Robot Arm 1 togoal: 11.0055107626801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.763138e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.855942e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-36.4788710511144;12.551815553713;1.41594877303585]
Robot Arm 1 end: [12.5527884962571;1.29060692014593;1.47976003462934]
Robot Arm 1 togoal: 9.07589982650625
Mobile Platform 2 Pose: [22.5498525337203;-6.21246450984179;0.312303973224718]
Robot Arm 2 end: [-6.77298327388844;0.389715221164498;1.0578327269649]
Robot Arm 1 togoal: 10.8891846917164

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.002779e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.444572e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-36.6868765913372;12.6786722843929;1.28867852587625]
Robot Arm 1 end: [12.2035716634336;1.82350165053843;0.758028222393219]
Robot Arm 1 togoal: 8.50252781636494
Mobile Platform 2 Pose: [23.0684133262074;-6.5073939873382;0.275269730964133]
Robot Arm 2 end: [-6.93444997359144;0.405181717672939;0.13153465409203]
Robot Arm 1 togoal: 10.9359964104643

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.881223e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.089448e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-36.1744717616214;12.414801102068;1.23457921266767]
Robot Arm 1 end: [12.4529443526669;1.7715817035867;0.290682045510949]
Robot Arm 1 togoal: 8.67395445729153
Mobile Platform 2 Pose: [23.0825565934541;-6.51057987545103;0.262641848565329]
Robot Arm 2 end: [-6.72516629603402;0.312292382391091;-0.0455332354477161]
Robot Arm 1 togoal: 10.932608853623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.056264e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.279700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-36.0806450707523;12.3660656494425;1.21115818495842]
Robot Arm 1 end: [12.2886742691734;1.42612800327738;0.15526659188756]
Robot Arm 1 togoal: 8.97855994403308
Mobile Platform 2 Pose: [22.1791706693128;-5.9390028362015;0.214937370290419]
Robot Arm 2 end: [-6.05755008803779;0.164284055681752;1.04500065561208]
Robot Arm 1 togoal: 10.9124771270677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.487530e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.003404e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-35.6931840082854;12.1099016820241;1.23682016072486]
Robot Arm 1 end: [12.1593808441836;1.60417759706634;0.359243385602022]
Robot Arm 1 togoal: 8.74656720564858
Mobile Platform 2 Pose: [22.322859272017;-6.03113552781505;0.192579542284155]
Robot Arm 2 end: [-6.10617562196757;0.036750524560854;1.08531251135837]
Robot Arm 1 togoal: 10.7744165952032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.932967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.520023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-35.1716497647341;11.8159571286886;1.18546120006835]
Robot Arm 1 end: [11.9331044910973;1.27457264092988;0.563969090180722]
Robot Arm 1 togoal: 8.9880967804235
Mobile Platform 2 Pose: [22.5131414533443;-6.16459099239886;0.172880853605608]
Robot Arm 2 end: [-6.3824844586469;-0.297847595152067;0.938316758657501]
Robot Arm 1 togoal: 10.3710028318658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.341422e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.618245e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-34.6375072107094;11.475446420691;1.05139610503626]
Robot Arm 1 end: [11.3299282012692;1.17908736162319;0.706229186252586]
Robot Arm 1 togoal: 8.95773745280255
Mobile Platform 2 Pose: [23.0792193532896;-6.54035380838315;0.0720520403662328]
Robot Arm 2 end: [-7.11238702330716;-0.392869656659573;0.327299790819246]
Robot Arm 1 togoal: 10.1024839681385

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.735434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.030003e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-34.4782648476105;11.376732587139;1.02873015102777]
Robot Arm 1 end: [11.4980724836929;1.27032957192698;0.770753896234307]
Robot Arm 1 togoal: 8.88899522811134
Mobile Platform 2 Pose: [23.7164603262394;-6.92341983538569;0.0029922466076882]
Robot Arm 2 end: [-6.90913951768747;-0.24361805612496;1.33686565570323]
Robot Arm 1 togoal: 10.2359324251698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.623125e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.475572e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-33.9363044964459;11.0555912661471;1.08429281695775]
Robot Arm 1 end: [11.1812989203512;1.14210498534965;0.629398146414092]
Robot Arm 1 togoal: 8.98068622629186
Mobile Platform 2 Pose: [23.8184165302915;-7.00513024249268;-0.00765226263408955]
Robot Arm 2 end: [-7.43523148535244;-0.654629436797226;0.788594796126677]
Robot Arm 1 togoal: 9.7185598665628

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004226e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.978047e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-33.9119165044866;11.0453162436829;1.13124003660025]
Robot Arm 1 end: [11.1830831928784;1.25470568853303;0.667326479623908]
Robot Arm 1 togoal: 8.86615006160167
Mobile Platform 2 Pose: [24.2339092029254;-7.37839416008585;-0.0851505711333649]
Robot Arm 2 end: [-7.39800691427722;0.268566987830758;-0.0942744248627549]
Robot Arm 1 togoal: 10.7155469981393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.940472e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.493553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-34.0093809751696;11.1125989872717;1.21551206683388]
Robot Arm 1 end: [11.1032839362428;1.5208099753164;0.786294937282603]
Robot Arm 1 togoal: 8.58217282789434
Mobile Platform 2 Pose: [23.2981524391106;-6.83768941672962;-0.0123242096845282]
Robot Arm 2 end: [-7.35569447157372;-0.15186284031583;0.208831484547422]
Robot Arm 1 togoal: 10.2810477800934

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.524133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.781775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-33.8719972308303;11.0280461586028;1.21269568427477]
Robot Arm 1 end: [10.9992373184223;1.47565618197738;0.726270093852688]
Robot Arm 1 togoal: 8.61942622046927
Mobile Platform 2 Pose: [23.7843975212944;-7.02399244814766;-0.156462140752299]
Robot Arm 2 end: [-7.41603754682694;-0.332320002387168;0.423825656409936]
Robot Arm 1 togoal: 10.0670099848904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011951e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.562085e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-33.6727127047549;10.901502704902;1.21830607968376]
Robot Arm 1 end: [10.88824843433;1.61081968931528;0.872309493442755]
Robot Arm 1 togoal: 8.46097694947619
Mobile Platform 2 Pose: [24.2851506745275;-7.25508050574557;-0.272691722534918]
Robot Arm 2 end: [-7.26542511402496;-0.674226591044096;0.205641747057612]
Robot Arm 1 togoal: 9.80704424461203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006356e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.629362e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-33.4263658649353;10.7568645847689;1.19825432982544]
Robot Arm 1 end: [10.8716580056799;1.57598177568403;1.04532983094079]
Robot Arm 1 togoal: 8.4823424142277
Mobile Platform 2 Pose: [24.7175859311428;-7.59694954737343;-0.466632537509827]
Robot Arm 2 end: [-8.20743041931179;-0.271964650683083;0.715759837331617]
Robot Arm 1 togoal: 9.9245347084255

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.693529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.699132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-33.2381263869328;10.6471785656552;1.16915673285688]
Robot Arm 1 end: [10.8474941300149;1.43908182911823;0.947423668772975]
Robot Arm 1 togoal: 8.62186499759114
Mobile Platform 2 Pose: [24.7145838794028;-7.59699055381705;-0.463023702157943]
Robot Arm 2 end: [-8.25536507034361;-0.570976474218187;0.593305433982899]
Robot Arm 1 togoal: 9.63383894900375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.827756e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.346515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-32.8198796138644;10.3938054132329;1.1220483123387]
Robot Arm 1 end: [10.4042742854897;1.32721937891853;0.769786861194318]
Robot Arm 1 togoal: 8.71463611284079
Mobile Platform 2 Pose: [25.0528273624066;-7.80674733453814;-0.467569438246301]
Robot Arm 2 end: [-8.24163196100767;-1.09003753990988;0.632622825771119]
Robot Arm 1 togoal: 9.12515770847035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.719092e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.193946e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-32.8059595967504;10.3918369387493;1.07676200396769]
Robot Arm 1 end: [10.4750769702425;1.05403053672633;0.75269019953149]
Robot Arm 1 togoal: 8.99146082207867
Mobile Platform 2 Pose: [25.7067050103825;-8.28631758396885;-0.511393845549097]
Robot Arm 2 end: [-8.91149084984829;-1.02727204749185;0.684497000111565]
Robot Arm 1 togoal: 9.07713811443384

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.686288e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.951964e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-33.0035015131652;10.5122838976094;0.999254059538996]
Robot Arm 1 end: [10.6672395690779;1.05828056646488;0.670937207475297]
Robot Arm 1 togoal: 9.00678421090668
Mobile Platform 2 Pose: [26.5420536817419;-8.806940708834;-0.539723304114614]
Robot Arm 2 end: [-8.86539517503408;-0.740232673857083;1.40809432286493]
Robot Arm 1 togoal: 9.32970346447674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.196587e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.689277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-33.0732655609089;10.5635099462248;1.0409835097914]
Robot Arm 1 end: [10.736281003536;1.34663335849668;0.665250902085092]
Robot Arm 1 togoal: 8.72669298572103
Mobile Platform 2 Pose: [26.6415321459888;-8.87926694628912;-0.547543707229422]
Robot Arm 2 end: [-9.45380362336595;-1.08139113492259;0.874200028850988]
Robot Arm 1 togoal: 8.95869771634046

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.205015e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.700828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-32.5954659742128;10.2975823615352;1.11845645027865]
Robot Arm 1 end: [10.8319854373197;1.57126013791784;0.669716599993905]
Robot Arm 1 togoal: 8.51237562949913
Mobile Platform 2 Pose: [27.5980589709533;-9.46252797026372;-0.607386356569961]
Robot Arm 2 end: [-10.1707769818774;-1.04772803271154;0.703241401136528]
Robot Arm 1 togoal: 8.99116606897309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.118026e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.449548e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-32.1967537470555;10.0609084645896;1.08026529615477]
Robot Arm 1 end: [10.6263224186266;1.46898280249392;0.566626261450535]
Robot Arm 1 togoal: 8.6070531210776
Mobile Platform 2 Pose: [28.0956654695523;-9.75501505078991;-0.638005919596472]
Robot Arm 2 end: [-9.96775975060199;-1.16643609225484;1.17505096393316]
Robot Arm 1 togoal: 8.84039430576797

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.271232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-32.0455428263343;9.97561330708939;1.09014115314589]
Robot Arm 1 end: [10.5689421549167;1.40029597143413;0.491096801301819]
Robot Arm 1 togoal: 8.67982172358753
Mobile Platform 2 Pose: [28.1923370486417;-9.83488641555767;-0.648254157303622]
Robot Arm 2 end: [-10.3715130855051;-1.25556661457213;0.563316741755468]
Robot Arm 1 togoal: 8.80456100143607

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.060806e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.915257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-32.18184321343;10.070222058798;1.13802068873149]
Robot Arm 1 end: [10.6928125704014;1.34930928289192;0.43942931028177]
Robot Arm 1 togoal: 8.74552653061115
Mobile Platform 2 Pose: [28.63376751888;-10.1714235130096;-0.683490130777973]
Robot Arm 2 end: [-10.6623299901266;-0.756182200520044;1.22501296536519]
Robot Arm 1 togoal: 9.27224119889444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.557306e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.069997e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-32.5417226533052;10.3091984375228;1.21143928595133]
Robot Arm 1 end: [11.0048409563181;1.23192995363234;0.363007435895215]
Robot Arm 1 togoal: 8.90110692352507
Mobile Platform 2 Pose: [28.4063547882818;-10.0318849710471;-0.676789468034312]
Robot Arm 2 end: [-10.3643807037847;-0.734848437590127;1.32384086894294]
Robot Arm 1 togoal: 9.27440987312271

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.573704e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.843375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-32.7456722508437;10.4661907229175;1.26493656187993]
Robot Arm 1 end: [11.0128393326399;1.70885038023747;0.990463990312718]
Robot Arm 1 togoal: 8.36961611954847
Mobile Platform 2 Pose: [28.5922557663338;-10.1443895061007;-0.691966527516259]
Robot Arm 2 end: [-10.5446573168403;-0.872123498770877;1.24100735221132]
Robot Arm 1 togoal: 9.14839750433344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.873615e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-32.903767512289;10.5604745132536;1.2834607783337]
Robot Arm 1 end: [11.2664948074738;1.57817012095052;0.833237525385959]
Robot Arm 1 togoal: 8.54425215162193
Mobile Platform 2 Pose: [29.0635006412843;-10.4103123671298;-0.748352219005135]
Robot Arm 2 end: [-10.2467613892881;-1.19396056135518;1.08803088329547]
Robot Arm 1 togoal: 8.8201294794482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.917452e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.110050e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-33.2284228094224;10.7657151631163;1.31571571675388]
Robot Arm 1 end: [11.5237427713596;1.09653421159171;0.654198725565282]
Robot Arm 1 togoal: 9.07440573966081
Mobile Platform 2 Pose: [29.077292233027;-10.4320808617494;-0.756575961596039]
Robot Arm 2 end: [-10.2138464876322;-1.32671705580241;0.826003071121674]
Robot Arm 1 togoal: 8.70371116717069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.360831e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.243630e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-33.3780711401376;10.9005967258361;1.35579007442603]
Robot Arm 1 end: [11.4975225918388;1.87190228630622;0.435985540004852]
Robot Arm 1 togoal: 8.3358144613961
Mobile Platform 2 Pose: [29.2515410950922;-10.5505170058241;-0.990206014171172]
Robot Arm 2 end: [-10.8107981177503;-1.01740682957382;0.601163462942423]
Robot Arm 1 togoal: 9.06589614478571

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.044333e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.263069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-33.4966589179586;10.9713980056694;1.37084497527947]
Robot Arm 1 end: [11.6140374473698;1.73765937669848;0.363977935721244]
Robot Arm 1 togoal: 8.4976520000644
Mobile Platform 2 Pose: [28.5352429601684;-10.053703988246;-1.12886996428732]
Robot Arm 2 end: [-10.2308307966098;-1.16238572259904;0.605171785490896]
Robot Arm 1 togoal: 8.88793847251857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.057402e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.239327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-33.6635127442217;11.0720152913893;1.38526493586944]
Robot Arm 1 end: [11.7834725286159;1.59392430592915;0.375481421585976]
Robot Arm 1 togoal: 8.66920386473106
Mobile Platform 2 Pose: [28.53950263769;-10.0545559237504;-1.48370125813706]
Robot Arm 2 end: [-10.1439746818486;-1.37979347170473;0.662810958280858]
Robot Arm 1 togoal: 8.66401625868769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.634840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.842635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-33.9066334590101;11.2297628045977;1.42074018773119]
Robot Arm 1 end: [11.9752263702947;1.47350387172403;0.722179136291299]
Robot Arm 1 togoal: 8.7886728356282
Mobile Platform 2 Pose: [28.9293487711089;-10.3045097525196;-1.4885057855324]
Robot Arm 2 end: [-10.4605803560888;-1.3981396297365;0.613906602785856]
Robot Arm 1 togoal: 8.66181011828325

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.034896e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.055211e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-34.2149142321489;11.4483731627007;1.4771274051631]
Robot Arm 1 end: [12.22242548808;1.39351052581565;0.520942068619731]
Robot Arm 1 togoal: 8.94488412364465
Mobile Platform 2 Pose: [29.8274689074128;-10.8099478483831;-1.64215200671059]
Robot Arm 2 end: [-11.2185840007862;-2.1271938327537;0.064457489834837]
Robot Arm 1 togoal: 8.09861346144646

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.804978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.887079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-34.5647608375334;11.6912220683852;1.51639664990082]
Robot Arm 1 end: [12.4528801190012;1.60482215948305;0.725125136627859]
Robot Arm 1 togoal: 8.78231453833474
Mobile Platform 2 Pose: [29.8306691708167;-10.8239213096162;-1.63676894095262]
Robot Arm 2 end: [-11.3233671706658;-2.03985459455049;0.0926352435466964]
Robot Arm 1 togoal: 8.1948423671095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.090496e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.468423e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-34.7856449850979;11.840577344969;1.54849374328381]
Robot Arm 1 end: [12.6263021348032;1.47510813429113;0.642331617763692]
Robot Arm 1 togoal: 8.96344255022827
Mobile Platform 2 Pose: [29.8036144020937;-10.800581200408;-1.63910108081761]
Robot Arm 2 end: [-10.8886173982202;-1.69694047120699;-0.164970209208906]
Robot Arm 1 togoal: 8.51897493630776

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.264952e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.819580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-34.9963455118123;12.0013506372022;1.59223404990341]
Robot Arm 1 end: [12.7146353356942;1.92936684162645;0.488803126240838]
Robot Arm 1 togoal: 8.57728365912172
Mobile Platform 2 Pose: [29.6502351373574;-10.6898777931936;-1.63726832841073]
Robot Arm 2 end: [-10.7008195377515;-1.64888674155586;-0.19837994550537]
Robot Arm 1 togoal: 8.55502823355141

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.269230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.575664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-35.1445533714968;12.0932677828489;1.611187025523]
Robot Arm 1 end: [12.81575028873;1.78523870314001;0.411862294542331]
Robot Arm 1 togoal: 8.75448109841695
Mobile Platform 2 Pose: [29.4903472092763;-10.5810354640046;-1.6297814401775]
Robot Arm 2 end: [-10.4865751810954;-1.66425809470321;-0.210160135771008]
Robot Arm 1 togoal: 8.5275004505577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.183407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.996841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-35.3143122194465;12.2046482658566;1.63718799653385]
Robot Arm 1 end: [12.9701408500201;1.65769055445379;0.432227528262267]
Robot Arm 1 togoal: 8.92195544986756
Mobile Platform 2 Pose: [29.2862468467393;-10.4328742772221;-1.61089030843175]
Robot Arm 2 end: [-10.2962216647421;-1.80088518673303;-0.114052036911301]
Robot Arm 1 togoal: 8.36580098730566

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.167658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.988931e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-35.4150157817377;12.2759230574056;1.65514975198347]
Robot Arm 1 end: [13.0435325905871;1.64837615544218;0.575452580090691]
Robot Arm 1 togoal: 8.93905875326085
Mobile Platform 2 Pose: [29.2914115088644;-10.4410143640147;-1.61337609130429]
Robot Arm 2 end: [-10.5791034986263;-1.93678281134731;-0.070068416956265]
Robot Arm 1 togoal: 8.23907343097154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.459524e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.322899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-35.5537872483112;12.3791273006614;1.68577122042758]
Robot Arm 1 end: [13.1443569028654;1.73234023391491;0.576628624756891]
Robot Arm 1 togoal: 8.89567398428686
Mobile Platform 2 Pose: [29.2933374101073;-10.4488794939026;-1.61240875030907]
Robot Arm 2 end: [-10.3191219416795;-1.9272242844665;-0.0727521954450037]
Robot Arm 1 togoal: 8.23477945226552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.622795e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.197309e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-35.7549187600392;12.5118699489261;1.70848025778521]
Robot Arm 1 end: [13.2734603654683;1.65878424188012;0.546528815522196]
Robot Arm 1 togoal: 9.01337989752881
Mobile Platform 2 Pose: [29.299526962572;-10.4570298508275;-1.6273810411743]
Robot Arm 2 end: [-10.2174845813778;-1.75178764874125;0.190966691452139]
Robot Arm 1 togoal: 8.35758907439467

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.601063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.300499e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-35.8204151010083;12.56441520729;1.72704871358573]
Robot Arm 1 end: [13.319645026385;1.72775302243474;0.546594568606984]
Robot Arm 1 togoal: 8.96658123834097
Mobile Platform 2 Pose: [28.8933586151144;-10.1232085067178;-1.68206792023411]
Robot Arm 2 end: [-9.95612502688876;-1.19120791273796;1.11083017423787]
Robot Arm 1 togoal: 8.81844557367349

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.558384e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.854625e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-35.9485696224427;12.6553778675172;1.74961089705422]
Robot Arm 1 end: [13.4072401410568;1.75665908041384;0.539542856779873]
Robot Arm 1 togoal: 8.97358417909643
Mobile Platform 2 Pose: [28.9901420835081;-10.181267788353;-1.67714240562789]
Robot Arm 2 end: [-9.89869278975998;-1.13988887993828;0.940875484178028]
Robot Arm 1 togoal: 8.8796608417521

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.578834e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.796825e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-36.0253376689828;12.7134744477327;1.76906492115076]
Robot Arm 1 end: [13.4595437225738;1.77898495333638;0.542701145386436]
Robot Arm 1 togoal: 8.97276990760421
Mobile Platform 2 Pose: [29.0483223552115;-10.2265818821504;-1.68217825717387]
Robot Arm 2 end: [-9.94582080522175;-1.06993460954253;0.609955654684226]
Robot Arm 1 togoal: 8.97658092271913

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.572624e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.731516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-36.1012761821934;12.7701690759444;1.78729766994026]
Robot Arm 1 end: [13.5095983100125;1.78966763719323;0.544254104662221]
Robot Arm 1 togoal: 8.98225307756657
Mobile Platform 2 Pose: [29.0119145924168;-10.2500678887579;-1.74611162826359]
Robot Arm 2 end: [-9.95723789092455;-1.78361413341967;0.282061289426871]
Robot Arm 1 togoal: 8.30937990677224

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.591323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.926873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-36.1507844168107;12.8097571857153;1.80315648123949]
Robot Arm 1 end: [13.5401128943239;1.79305628902712;0.546907709818975]
Robot Arm 1 togoal: 8.99083867998006
Mobile Platform 2 Pose: [28.9637151310227;-10.2512043840903;-1.79143237896006]
Robot Arm 2 end: [-10.0194074172371;-1.87506337344669;0.30341757402457]
Robot Arm 1 togoal: 8.21568492564043

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.599169e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.791982e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-36.1858769931225;12.8399090625499;1.81737969771828]
Robot Arm 1 end: [13.5589556267203;1.79170541415077;0.548357978931019]
Robot Arm 1 togoal: 8.99934984663863
Mobile Platform 2 Pose: [28.9892164427661;-10.2842167758311;-1.78204044479609]
Robot Arm 2 end: [-10.0969368914299;-1.9469983957822;0.184246592481766]
Robot Arm 1 togoal: 8.16377004030406

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.605952e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.199939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-36.2096711211864;12.8627218753685;1.83035065635226]
Robot Arm 1 end: [13.5683439758182;1.78767442283761;0.548448347890887]
Robot Arm 1 togoal: 9.00673230595277
Mobile Platform 2 Pose: [28.8872974351888;-10.2060723220686;-1.74457932238884]
Robot Arm 2 end: [-10.1323584128228;-2.1247333898454;-0.00615294349647272]
Robot Arm 1 togoal: 8.02306294648688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.610390e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.109790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-36.2240964480595;12.8795012058299;1.84236549162362]
Robot Arm 1 end: [13.5697114950952;1.78194156613639;0.547682419200515]
Robot Arm 1 togoal: 9.0125840741546
Mobile Platform 2 Pose: [28.7406739018097;-10.1165953187966;-1.68925738969388]
Robot Arm 2 end: [-10.0046449728652;-1.57586331730787;-0.121268042286024]
Robot Arm 1 togoal: 8.58272362057442

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.613201e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.099862e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-36.2312141231829;12.8915890161341;1.85365127821383]
Robot Arm 1 end: [13.5645508197328;1.77530844723279;0.546027246664897]
Robot Arm 1 togoal: 9.01677023963482
Mobile Platform 2 Pose: [28.7352361945893;-10.1304542045548;-1.72611791786639]
Robot Arm 2 end: [-10.2277681673512;-1.4339417402874;0.0975550031437748]
Robot Arm 1 togoal: 8.68650839554789

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.830945e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.771164e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-36.233164652902;12.9003534007709;1.86438709983628]
Robot Arm 1 end: [13.5545336615524;1.76900961183038;0.543216691319086]
Robot Arm 1 togoal: 9.01886758523403
Mobile Platform 2 Pose: [28.5017840275068;-9.94369247030182;-2.00963565833492]
Robot Arm 2 end: [-10.2426889635719;-1.72664208268375;0.218694983157948]
Robot Arm 1 togoal: 8.37874379116291

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991329e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.833170e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-36.1966258149056;12.8893531964525;1.874739378953]
Robot Arm 1 end: [13.5235875209891;1.75680167958583;0.536053572539442]
Robot Arm 1 togoal: 9.0186532826535
Mobile Platform 2 Pose: [28.0868263800328;-9.69454631111048;-2.03028048889214]
Robot Arm 2 end: [-9.97911853658048;-1.67577839821974;0.208127041579381]
Robot Arm 1 togoal: 8.42714285603441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996420e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.289225e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-36.1816575183855;12.8895610493569;1.88397669522879]
Robot Arm 1 end: [13.5044372694291;1.76241483027752;0.528464643957545]
Robot Arm 1 togoal: 9.00688716687344
Mobile Platform 2 Pose: [27.3734035431621;-9.28009109541623;-2.02154345056303]
Robot Arm 2 end: [-9.47588492133215;-1.58198526947064;0.264898324000504]
Robot Arm 1 togoal: 8.5273360458906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991488e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.179277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-36.1712646394813;12.8914644077882;1.8938709684616]
Robot Arm 1 end: [13.4872837156032;1.76663437512446;0.530780186113293]
Robot Arm 1 togoal: 8.99610985403176
Mobile Platform 2 Pose: [26.3980287084625;-8.7430391016207;-2.0824843907464]
Robot Arm 2 end: [-8.74230666853407;-1.79645562232064;0.162523180715255]
Robot Arm 1 togoal: 8.40983900803711

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.991205e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.615979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-36.1682393458366;12.8961448032981;1.90356776485749]
Robot Arm 1 end: [13.4690249498256;1.76048822361706;0.532664454078924]
Robot Arm 1 togoal: 8.99447027717569
Mobile Platform 2 Pose: [26.7568330784394;-8.97564258401353;-2.17411200801182]
Robot Arm 2 end: [-8.80225067126171;-2.02470796950185;0.19594387414288]
Robot Arm 1 togoal: 8.17286119806838

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999500e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.452223e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-36.188299214934;12.9128242752691;1.91194738014366]
Robot Arm 1 end: [13.4634696286242;1.76492448821392;0.524163008713646]
Robot Arm 1 togoal: 8.98920322055396
Mobile Platform 2 Pose: [27.1037939826746;-9.22195692807172;-2.21688185816728]
Robot Arm 2 end: [-8.93603229296417;-2.23424065862409;0.267467528100166]
Robot Arm 1 togoal: 7.93790835723679

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.917415e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-36.2135499431703;12.9320877354765;1.92119205159214]
Robot Arm 1 end: [13.4631983785752;1.7743328345553;0.52442847833089]
Robot Arm 1 togoal: 8.98045088644458
Mobile Platform 2 Pose: [27.0765739110163;-9.2226282141061;-2.24692260163914]
Robot Arm 2 end: [-8.9938955331248;-2.2023129159391;0.177185851505759]
Robot Arm 1 togoal: 7.97634042182816

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998391e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.150762e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-36.2312903950828;12.9461927589788;1.93143756679978]
Robot Arm 1 end: [13.4520138662304;1.7668006963276;0.530493984735342]
Robot Arm 1 togoal: 8.98237566977213
Mobile Platform 2 Pose: [27.0415404660168;-9.20835426426835;-2.26659630706826]
Robot Arm 2 end: [-9.00923703678526;-2.11212746023462;0.119987961284599]
Robot Arm 1 togoal: 8.07235896039617

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000350e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.993051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-36.2710449376178;12.9716344103928;1.93902938393489]
Robot Arm 1 end: [13.4494175192749;1.76525425804074;0.516154846575211]
Robot Arm 1 togoal: 8.98438826183276
Mobile Platform 2 Pose: [27.557042515149;-9.59147056831324;-2.35534573199571]
Robot Arm 2 end: [-9.45777288525227;-2.18988867517822;0.135706231014644]
Robot Arm 1 togoal: 7.95052752818747

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.999763e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.090539e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-36.3192218266302;13.0023971187223;1.94705664663564]
Robot Arm 1 end: [13.4639577666822;1.7887167911841;0.509739991884085]
Robot Arm 1 togoal: 8.96921524709686
Mobile Platform 2 Pose: [27.7776622765437;-9.78858945554814;-2.50911247771024]
Robot Arm 2 end: [-9.90302292165781;-2.66436426794862;0.157595137832147]
Robot Arm 1 togoal: 7.46189178193142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.994472e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.742434e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-36.314193444608;13.0028784761279;1.96165007655323]
Robot Arm 1 end: [13.4413474135126;1.7835482943981;0.534461630559568]
Robot Arm 1 togoal: 8.96248897379275
Mobile Platform 2 Pose: [27.753667525142;-9.74946430991209;-2.56172478556731]
Robot Arm 2 end: [-9.86381136465621;-2.81529550656251;0.16862970599607]
Robot Arm 1 togoal: 7.31214273756482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002457e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.611946e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-36.3520820807088;13.025334630813;1.96865870292616]
Robot Arm 1 end: [13.4099223034876;1.74606142006248;0.500486334408182]
Robot Arm 1 togoal: 8.98868846600342
Mobile Platform 2 Pose: [27.9495534621218;-9.8947485158967;-2.57127807269117]
Robot Arm 2 end: [-9.89379099395558;-2.9231821614096;0.0399504390827956]
Robot Arm 1 togoal: 7.23091549359065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.718117e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-36.4542761421135;13.0841181455994;1.96641358181651]
Robot Arm 1 end: [13.4754675938058;1.8386658957042;0.455198985101983]
Robot Arm 1 togoal: 8.93432600484747
Mobile Platform 2 Pose: [27.9583882926796;-9.90002163543805;-2.57515374204823]
Robot Arm 2 end: [-9.96944017370195;-2.8447040130654;0.0725813372191771]
Robot Arm 1 togoal: 7.30048705130812

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956483e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.602602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-36.2476054194195;12.9695766294581;2.00855192431574]
Robot Arm 1 end: [13.4169182554513;1.96593896063823;0.489974452394647]
Robot Arm 1 togoal: 8.7911592425824
Mobile Platform 2 Pose: [27.9637125450687;-9.90171239749985;-2.57901978372396]
Robot Arm 2 end: [-10.0319545077442;-2.72041676771795;0.114353013491619]
Robot Arm 1 togoal: 7.41431108539021

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.531678e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.731106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-35.9033064824085;12.7684924325166;2.03816677576339]
Robot Arm 1 end: [13.1511542215793;1.80106082537292;0.543779291194276]
Robot Arm 1 togoal: 8.83783552863089
Mobile Platform 2 Pose: [27.9085231619067;-9.86045895090598;-2.58433860911909]
Robot Arm 2 end: [-10.3149577965604;-2.72544153881148;0.497106085702154]
Robot Arm 1 togoal: 7.35301012964077

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.609251e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.053192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-35.8692709229344;12.7567475900729;2.06618597827736]
Robot Arm 1 end: [13.0705647162095;1.83377219685238;0.507076791432427]
Robot Arm 1 togoal: 8.7831477547747
Mobile Platform 2 Pose: [27.5127598043146;-9.61064308390386;-2.57474524714432]
Robot Arm 2 end: [-9.9132130081143;-2.75183340517609;0.533736114325996]
Robot Arm 1 togoal: 7.31560940375017

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.920130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.063363e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-36.1048081404472;12.8931269434553;2.05516981271663]
Robot Arm 1 end: [13.1573571848029;1.99550940683907;0.566900700141703]
Robot Arm 1 togoal: 8.65742915202997
Mobile Platform 2 Pose: [27.2565050860309;-9.43794182392337;-2.57032822067622]
Robot Arm 2 end: [-9.36742732131318;-2.53977125819868;0.976589361683382]
Robot Arm 1 togoal: 7.50676654871669

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.571786e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.594905e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-36.0749984141537;12.8898423760741;2.12665825608531]
Robot Arm 1 end: [13.1499022939721;2.06118463952916;0.568193093964885]
Robot Arm 1 togoal: 8.59386495061485
Mobile Platform 2 Pose: [27.2980776977803;-9.46286289254125;-2.5669699470781]
Robot Arm 2 end: [-9.55219749248469;-2.58690656293286;1.06557755744286]
Robot Arm 1 togoal: 7.440557169568

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.867563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.678936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-36.4003473611562;13.0727666668298;2.07936143098546]
Robot Arm 1 end: [13.0589236289445;2.04298764603042;0.595695630495121]
Robot Arm 1 togoal: 8.57480306159648
Mobile Platform 2 Pose: [27.4899378495529;-9.58802523314006;-2.56419267744029]
Robot Arm 2 end: [-9.7929985909684;-2.51328424179851;0.976537087964641]
Robot Arm 1 togoal: 7.50934100238

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.316896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.256815e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-36.2064689831014;12.9369301377737;2.03425080377319]
Robot Arm 1 end: [12.8492120559165;2.00629713115576;0.61556989565194]
Robot Arm 1 togoal: 8.53446533584901
Mobile Platform 2 Pose: [27.7164901000737;-9.73079852964064;-2.57286596411179]
Robot Arm 2 end: [-10.1035310304574;-2.54288468417359;0.9122018647681]
Robot Arm 1 togoal: 7.48264142380632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.576381e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.760055e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-35.8804209367261;12.6933286800446;2.09155949849766]
Robot Arm 1 end: [12.6286587503526;2.09808020975508;0.623417136496745]
Robot Arm 1 togoal: 8.37590820115193
Mobile Platform 2 Pose: [27.5412438610999;-9.62357540110017;-2.56544872879345]
Robot Arm 2 end: [-9.8680938015486;-2.52710723037818;0.978545081812686]
Robot Arm 1 togoal: 7.49371622959674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.112660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.192116e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-35.7035763249297;12.5714240572697;2.08301801799204]
Robot Arm 1 end: [12.493492826586;2.11033317769949;0.711025711174799]
Robot Arm 1 togoal: 8.31386837709757
Mobile Platform 2 Pose: [27.6711185374098;-9.70340114012182;-2.57513973741844]
Robot Arm 2 end: [-10.0576875806667;-2.61220390678651;0.970202778465216]
Robot Arm 1 togoal: 7.40852458665786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.401336e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.754680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-35.6872218711907;12.5359812957344;2.07042101101369]
Robot Arm 1 end: [12.6014911644593;2.32480666576171;0.738491583422062]
Robot Arm 1 togoal: 8.14178533361763
Mobile Platform 2 Pose: [27.9840099607741;-9.88431656112646;-2.61272349241725]
Robot Arm 2 end: [-10.1579469988526;-3.02082305095918;0.885140056850535]
Robot Arm 1 togoal: 7.00986276668604

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.189110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.898602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-35.4222496030801;12.3511437899227;2.10459747685004]
Robot Arm 1 end: [12.6098940696602;2.08877018126356;0.70027368708305]
Robot Arm 1 togoal: 8.37094355377135
Mobile Platform 2 Pose: [28.1496391370804;-9.99913469206447;-2.62645539265183]
Robot Arm 2 end: [-10.2128725311448;-3.12920758058874;0.911773518301821]
Robot Arm 1 togoal: 6.90103326257435

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.032204e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.082776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-35.46634795701;12.3779706997458;2.10034668037354]
Robot Arm 1 end: [12.6665517049007;2.16939881733882;0.689812230148818]
Robot Arm 1 togoal: 8.31382499127531
Mobile Platform 2 Pose: [28.3851065340433;-10.1621530011282;-2.64755363349578]
Robot Arm 2 end: [-10.1258321315445;-3.19632810833708;1.05065615401011]
Robot Arm 1 togoal: 6.8210709033255

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.768058e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-35.414900296579;12.3523345009644;2.12442496855955]
Robot Arm 1 end: [12.7036172508272;2.09785237364835;0.810652449212049]
Robot Arm 1 togoal: 8.38200912613056
Mobile Platform 2 Pose: [28.6661861247007;-10.365309360539;-2.75086337202538]
Robot Arm 2 end: [-10.367085083042;-3.22165629381696;1.19872890623501]
Robot Arm 1 togoal: 6.79592182968725

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.036252e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.039021e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-35.4619727978983;12.3897453983352;2.14174526594892]
Robot Arm 1 end: [12.7798311466777;2.12227486774242;0.782035001877114]
Robot Arm 1 togoal: 8.38642258247776
Mobile Platform 2 Pose: [29.2065158371422;-10.6894139257354;-2.94430089757512]
Robot Arm 2 end: [-11.1888090216636;-2.75331573251062;1.10563709266813]
Robot Arm 1 togoal: 7.35528558979554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.937137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.683947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-35.5410706247029;12.4418231482254;2.14716441165993]
Robot Arm 1 end: [12.9170536098126;2.33038084150175;0.88535258756338]
Robot Arm 1 togoal: 8.2302070102678
Mobile Platform 2 Pose: [29.2544923563407;-10.7092860697003;-2.9492252499719]
Robot Arm 2 end: [-10.8901609510491;-2.97232525060735;1.40857531396122]
Robot Arm 1 togoal: 7.08471864028873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001332e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.836156e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-35.535779606111;12.4438340480875;2.16424301536457]
Robot Arm 1 end: [12.9554326165713;2.22566708111176;0.902657612393389]
Robot Arm 1 togoal: 8.34009481919204
Mobile Platform 2 Pose: [29.5225579509329;-10.7972883528006;-3.18322557557028]
Robot Arm 2 end: [-10.8920394896434;-3.29585179081875;1.41445499968011]
Robot Arm 1 togoal: 6.76407343978794

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.861044e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.865694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-35.7183178192011;12.5649526232311;2.18762041626387]
Robot Arm 1 end: [13.1531143438016;2.28641327821484;0.854504008096459]
Robot Arm 1 togoal: 8.35977075563747
Mobile Platform 2 Pose: [29.6030335689614;-10.7567777341464;-3.45698326847921]
Robot Arm 2 end: [-10.6987241465345;-3.40634048567949;0.412573189728776]
Robot Arm 1 togoal: 6.72258664633396

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.967721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.037680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-35.7615990232019;12.5952503711133;2.19738809696131]
Robot Arm 1 end: [13.2452343097481;2.28993557416607;0.8841734135664]
Robot Arm 1 togoal: 8.38940923288791
Mobile Platform 2 Pose: [29.660675108292;-10.7891384442366;-3.49121465008728]
Robot Arm 2 end: [-10.6950795366409;-3.25778681801828;0.0248244481404705]
Robot Arm 1 togoal: 6.94111773675733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.913323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.771141e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-35.8394508249683;12.6502919281111;2.21514981488818]
Robot Arm 1 end: [13.3241249783132;2.27777779657962;0.78401514072485]
Robot Arm 1 togoal: 8.439530159508
Mobile Platform 2 Pose: [28.7472097797203;-10.252891987929;-3.47903571653461]
Robot Arm 2 end: [-9.7941829150578;-3.18599823259977;0.982745540856202]
Robot Arm 1 togoal: 6.83832571770135

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.541966e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.121074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-35.8918201555518;12.6869997009551;2.22542220424736]
Robot Arm 1 end: [13.3916357529378;2.23717416970827;0.873830283653006]
Robot Arm 1 togoal: 8.49608654580488
Mobile Platform 2 Pose: [28.3612008876708;-9.97869651167154;-3.47107112020629]
Robot Arm 2 end: [-9.37000800605542;-3.09390251567186;0.965103402150512]
Robot Arm 1 togoal: 6.95701756523429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.185056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.658132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-35.9618286783144;12.7370109508266;2.23881403317194]
Robot Arm 1 end: [13.4229480059103;2.17059439616511;0.800599036253845]
Robot Arm 1 togoal: 8.57526341922467
Mobile Platform 2 Pose: [27.7782454438328;-9.6500535177332;-3.50919581637513]
Robot Arm 2 end: [-9.43651894578626;-2.81209026916082;0.72097940973066]
Robot Arm 1 togoal: 7.25421188979239

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.506773e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004623e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-36.0186286436324;12.7789867573902;2.249563262438]
Robot Arm 1 end: [13.4638283560362;2.30177076111702;0.784067584089116]
Robot Arm 1 togoal: 8.47371876333489
Mobile Platform 2 Pose: [27.7053731926919;-9.61047422708144;-3.51573732584067]
Robot Arm 2 end: [-9.31500863270859;-2.92523017933574;0.397446318629814]
Robot Arm 1 togoal: 7.19610686854817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.961816e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.106261e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-36.0020892381232;12.7715054349895;2.25383367851848]
Robot Arm 1 end: [13.4440895685997;2.23509050528312;0.982113644427427]
Robot Arm 1 togoal: 8.51151989493674
Mobile Platform 2 Pose: [28.0998478931415;-9.83614948384298;-3.49648316567292]
Robot Arm 2 end: [-9.44147495344027;-3.30187356172054;0.143762691840864]
Robot Arm 1 togoal: 6.86102256327716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002665e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.071843e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-35.9806529030504;12.7613864431994;2.25826611462957]
Robot Arm 1 end: [13.340294765104;1.9608765680713;0.968099741474481]
Robot Arm 1 togoal: 8.72300254312952
Mobile Platform 2 Pose: [28.2116374006268;-9.87334815227276;-3.45295785467884]
Robot Arm 2 end: [-9.91626880148235;-3.46631004892913;0.0612878819991637]
Robot Arm 1 togoal: 6.69528750374232

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001317e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.476074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-35.9689574438171;12.758377089005;2.26257378977701]
Robot Arm 1 end: [13.3718161529479;2.10675960167283;0.966071413983585]
Robot Arm 1 togoal: 8.60118212169947
Mobile Platform 2 Pose: [28.195796660447;-9.85794531479955;-3.50686974939402]
Robot Arm 2 end: [-10.0649167799935;-3.61200398181474;0.411868332591461]
Robot Arm 1 togoal: 6.48389392050209

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000287e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-35.8688337630384;12.709022107097;2.27879389809017]
Robot Arm 1 end: [13.3272070512902;2.28003508028406;0.911432161118897]
Robot Arm 1 togoal: 8.42850746419545
Mobile Platform 2 Pose: [28.4845123901774;-9.96573959363639;-3.69840998324596]
Robot Arm 2 end: [-9.90696051299736;-3.27181054889847;0.35292402331578]
Robot Arm 1 togoal: 6.82946492218551

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.983475e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.689993e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-35.7959380128498;12.6734085811004;2.29335162061987]
Robot Arm 1 end: [13.2555669908643;2.22272273553608;1.01338318792511]
Robot Arm 1 togoal: 8.44644498639837
Mobile Platform 2 Pose: [28.4655943799072;-9.94112473866592;-3.65477038178606]
Robot Arm 2 end: [-9.78818738098125;-3.83879682312862;0.558968875552197]
Robot Arm 1 togoal: 6.23945454800913

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001296e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.536287e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-35.8217575464247;12.6916410677519;2.29694308512543]
Robot Arm 1 end: [13.2741283599428;2.429176675338;0.995460800436691]
Robot Arm 1 togoal: 8.26519653613305
Mobile Platform 2 Pose: [28.4889391230996;-9.96078973974962;-3.65519049143789]
Robot Arm 2 end: [-9.75013987118047;-3.80769301758509;0.536605850763826]
Robot Arm 1 togoal: 6.27504005481148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000708e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.150376e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-35.7966296471078;12.6809623469451;2.30720121214768]
Robot Arm 1 end: [13.2771135565846;2.25018856417528;1.0410081060513]
Robot Arm 1 togoal: 8.42789669916798
Mobile Platform 2 Pose: [28.440459025037;-9.92762695948028;-3.67293376115427]
Robot Arm 2 end: [-9.71631878609925;-3.72464454127325;0.534870225039739]
Robot Arm 1 togoal: 6.35869586452718

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000337e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.036327e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-35.7908394895949;12.6799558569319;2.31113851263165]
Robot Arm 1 end: [13.2637000793817;2.41477043947926;1.07239362558692]
Robot Arm 1 togoal: 8.26974566556421
Mobile Platform 2 Pose: [28.2680066019448;-9.81124829971781;-3.70302393206012]
Robot Arm 2 end: [-9.74734638861482;-3.59782138666662;0.622409407553058]
Robot Arm 1 togoal: 6.46986784232904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.956552e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.566494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-35.7214349969716;12.6473734111941;2.33101491234159]
Robot Arm 1 end: [13.2724614015189;2.64520773379101;0.790162252258801]
Robot Arm 1 togoal: 8.0830747068824
Mobile Platform 2 Pose: [28.7997467959602;-10.1099462176196;-3.49507008946729]
Robot Arm 2 end: [-10.0894604855205;-3.43606966400671;0.694475473228503]
Robot Arm 1 togoal: 6.61636814480553

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.874762e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.866801e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-35.8681829143993;12.7441509921546;2.37121223833694]
Robot Arm 1 end: [13.1396133678808;2.9568945373665;0.472507440473536]
Robot Arm 1 togoal: 7.78214901585265
Mobile Platform 2 Pose: [28.6150704533071;-9.99465251755813;-3.58526458565057]
Robot Arm 2 end: [-10.2689929490244;-3.32816406634662;0.059776980095098]
Robot Arm 1 togoal: 6.83527063444552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.943714e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.575017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-36.3321420479439;13.0044411240416;2.41945531692986]
Robot Arm 1 end: [13.2379284625892;2.31361290465147;0.0202383632318781]
Robot Arm 1 togoal: 8.47449189030118
Mobile Platform 2 Pose: [28.6497531546316;-10.0137688776289;-3.59885934582097]
Robot Arm 2 end: [-10.4090371614491;-3.70398631358933;0.00157842953238901]
Robot Arm 1 togoal: 6.4896642168697

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.110281e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.265236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-36.6752306146805;13.2018347814264;2.35333038935984]
Robot Arm 1 end: [13.5736528694842;2.17560977742891;0.276917399263233]
Robot Arm 1 togoal: 8.69136460540545
Mobile Platform 2 Pose: [28.7739018871986;-10.0880493521886;-3.60863386759092]
Robot Arm 2 end: [-10.573473369541;-3.9717236950643;0.25363843190213]
Robot Arm 1 togoal: 6.18669477657605

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.256054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.794441e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-35.8443658627763;12.7275490528939;2.41391086750436]
Robot Arm 1 end: [13.2120695662362;2.20963642228958;0.356755609032871]
Robot Arm 1 togoal: 8.506622160009
Mobile Platform 2 Pose: [28.7334137696734;-10.0823368539907;-3.59250691932056]
Robot Arm 2 end: [-10.6068980096647;-3.76503589563244;0.291865295507502]
Robot Arm 1 togoal: 6.38387615001065

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.377755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.525150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-35.3485085708866;12.4861792112535;2.48506439506628]
Robot Arm 1 end: [12.9795153270368;2.24257029310292;0.435505012694584]
Robot Arm 1 togoal: 8.38054452938243
Mobile Platform 2 Pose: [28.5403094974016;-9.96149310577188;-3.58738122129539]
Robot Arm 2 end: [-10.393988592194;-3.64572757097206;0.109301912046133]
Robot Arm 1 togoal: 6.52111160804784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.669214e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-35.3204028556289;12.4681840120798;2.48544807866916]
Robot Arm 1 end: [12.9890400624331;2.12429281790002;0.56270000361432]
Robot Arm 1 togoal: 8.47817568796991
Mobile Platform 2 Pose: [28.6000056066096;-9.99762142419473;-3.59046273437393]
Robot Arm 2 end: [-10.5900213167721;-3.88082222591049;0.304644724619261]
Robot Arm 1 togoal: 6.26673614739787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.785440e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.383452e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-35.6653235330184;12.6682001165845;2.45844296938058]
Robot Arm 1 end: [13.3018609251983;2.19968151675842;0.750154972889114]
Robot Arm 1 togoal: 8.50537806715098
Mobile Platform 2 Pose: [28.804730322212;-10.1217132004807;-3.60098912408694]
Robot Arm 2 end: [-10.7249279209349;-4.01665917253717;0.651146606976917]
Robot Arm 1 togoal: 6.08954291170432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.469060e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-36.1357144987113;12.9267826816425;2.42064012904192]
Robot Arm 1 end: [13.2289823811516;2.61365148220854;1.35675366712826]
Robot Arm 1 togoal: 8.06296772298696
Mobile Platform 2 Pose: [29.0964366010133;-10.3035223860652;-3.61597376043436]
Robot Arm 2 end: [-10.7860926379738;-4.1914062809611;0.800314972618134]
Robot Arm 1 togoal: 5.90568281717087

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000139e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011122e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-36.398991728205;13.0850596562826;2.40928392534098]
Robot Arm 1 end: [13.1693285641572;2.59272460921337;1.44814520019616]
Robot Arm 1 togoal: 8.05715086642856
Mobile Platform 2 Pose: [29.4363183644976;-10.5345044767601;-3.6394051128137]
Robot Arm 2 end: [-11.0056864442994;-4.21351967263015;0.35800713663546]
Robot Arm 1 togoal: 5.98726249662754

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000879e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012976e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-35.994237188187;12.835560488478;2.42510055384501]
Robot Arm 1 end: [12.8512642795975;2.60498661221137;1.41992864559077]
Robot Arm 1 togoal: 7.9262946208404
Mobile Platform 2 Pose: [29.5764216177233;-10.6386267465634;-3.64622042825146]
Robot Arm 2 end: [-11.2350808829665;-4.16504280188506;0.696194362690955]
Robot Arm 1 togoal: 6.02100113192478

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.729781e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.769550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-36.3236530652105;13.0699746142221;2.21059769777892]
Robot Arm 1 end: [12.576687222139;1.78819366313537;1.18278840371791]
Robot Arm 1 togoal: 8.61321469828568
Mobile Platform 2 Pose: [30.0090460441772;-10.9001311794117;-3.67722667263132]
Robot Arm 2 end: [-11.1583841564721;-4.38563960090533;0.455090903045107]
Robot Arm 1 togoal: 5.83087118258148

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013648e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.274147e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-36.2278448590956;13.0198040995888;2.15036124001393]
Robot Arm 1 end: [12.6077648920308;1.54943914289482;1.00228212542306]
Robot Arm 1 togoal: 8.85897763656463
Mobile Platform 2 Pose: [30.3200546996876;-11.1537722180283;-3.72639199571146]
Robot Arm 2 end: [-11.9355212256507;-3.80326784815281;0.636549809597503]
Robot Arm 1 togoal: 6.55194504830315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.507094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.036799e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-36.4107437967117;13.0357186925536;2.18601277491705]
Robot Arm 1 end: [12.6516363078194;1.52592610746662;0.940717859937354]
Robot Arm 1 togoal: 8.89819255844339
Mobile Platform 2 Pose: [30.1938514557727;-11.0827795347316;-3.71963469790615]
Robot Arm 2 end: [-11.8537891239564;-3.62164272675462;0.776141584035841]
Robot Arm 1 togoal: 6.68392023299904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.292206e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.697029e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-36.5395348764638;13.049941782824;2.20715418405865]
Robot Arm 1 end: [13.0695073262105;1.54794101483465;0.126021507946314]
Robot Arm 1 togoal: 9.09973303496683
Mobile Platform 2 Pose: [30.1970678924041;-11.0852929562286;-3.71867736247593]
Robot Arm 2 end: [-11.8387298665465;-3.64584780945252;0.779865633318718]
Robot Arm 1 togoal: 6.6562345009957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.572790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.157800e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-36.6523560892728;13.1164931794409;2.18165582202646]
Robot Arm 1 end: [13.2824232206964;1.75237968231772;0.047616652194043]
Robot Arm 1 togoal: 8.99824436176328
Mobile Platform 2 Pose: [30.3718060901734;-11.1821211893814;-3.72976946572455]
Robot Arm 2 end: [-11.9414255907014;-3.83437368502125;0.647264423987422]
Robot Arm 1 togoal: 6.52284409111384

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.992425e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-36.6608347279967;13.1064662773346;2.17022796343579]
Robot Arm 1 end: [13.2138697163708;2.04861227399684;-0.0331075120293666]
Robot Arm 1 togoal: 8.71600685417268
Mobile Platform 2 Pose: [30.5841956606578;-11.3047213802494;-3.73837794443841]
Robot Arm 2 end: [-11.9521669265561;-4.09074253424755;0.53490469092306]
Robot Arm 1 togoal: 6.30100495669333

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.819137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.864532e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-36.455872143048;12.9453982952166;2.11545408145638]
Robot Arm 1 end: [13.0073814404329;2.12936044599995;-0.0859612573035]
Robot Arm 1 togoal: 8.57750748176444
Mobile Platform 2 Pose: [31.1815066761228;-11.6163049849705;-3.7951756482252]
Robot Arm 2 end: [-11.5268417738576;-4.43544579504779;0.265568854173755]
Robot Arm 1 togoal: 5.90521944250664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.702870e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.604000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-36.1139625290669;12.7119208044613;2.02502680124366]
Robot Arm 1 end: [12.7773796372454;2.12214836138015;-0.0666839648875676]
Robot Arm 1 togoal: 8.5026539657834
Mobile Platform 2 Pose: [31.57200613002;-11.8527475748503;-4.01047552560255]
Robot Arm 2 end: [-11.7742472017395;-4.29344269228918;1.32851443853603]
Robot Arm 1 togoal: 5.9791136575951

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.382184e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.062769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-36.1233114580612;12.7184660477845;1.95257619719253]
Robot Arm 1 end: [12.6946358750602;1.71804861586187;0.342792971301557]
Robot Arm 1 togoal: 8.78862634493373
Mobile Platform 2 Pose: [31.6017070487767;-11.8476564416976;-4.09681132738958]
Robot Arm 2 end: [-12.0132079150122;-4.76625611681481;0.174916279074594]
Robot Arm 1 togoal: 5.76689019555483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.061728e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.299200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-36.2220470502157;12.7206335823451;1.76223122533878]
Robot Arm 1 end: [12.5387014469401;1.48979540708181;0.31473030039434]
Robot Arm 1 togoal: 8.96234756289937
Mobile Platform 2 Pose: [32.0707635483458;-12.0927136155195;-4.18675718638361]
Robot Arm 2 end: [-12.0480683831721;-4.47354297512242;1.30061769661639]
Robot Arm 1 togoal: 5.8978707776292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.894467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.966319e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-36.3063629743232;12.7975010105896;1.65483426046082]
Robot Arm 1 end: [12.5071974376623;1.42255937690087;0.33589990860023]
Robot Arm 1 togoal: 9.01459865223335
Mobile Platform 2 Pose: [32.0674075133347;-12.0589822937065;-4.24945453249258]
Robot Arm 2 end: [-12.1366158887451;-4.96743467558378;0.805307949582676]
Robot Arm 1 togoal: 5.51398733328117

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.740198e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006386e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-36.0539422714447;12.6134192768835;1.48530172099732]
Robot Arm 1 end: [12.4191892105136;1.11640887743297;0.359642013540341]
Robot Arm 1 togoal: 9.28005495899301
Mobile Platform 2 Pose: [32.3568634702665;-12.0382528868075;-4.28285572890515]
Robot Arm 2 end: [-11.8862248505917;-4.63646994824675;1.13464088661499]
Robot Arm 1 togoal: 5.69864652067011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.685513e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.295602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-36.1287494065216;12.5629845964155;1.24164878008552]
Robot Arm 1 end: [12.3325359207391;0.800684846770494;0.420398162059656]
Robot Arm 1 togoal: 9.55402781635337
Mobile Platform 2 Pose: [32.2914259078521;-11.9339112422518;-4.32724520256964]
Robot Arm 2 end: [-11.7309944326251;-4.0012113981311;0.598376351023496]
Robot Arm 1 togoal: 6.31134223547907

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.019223e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.692927e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-36.206243700935;12.5597838293926;1.1455903653893]
Robot Arm 1 end: [12.2891639669211;0.778453748751325;0.286532577050119]
Robot Arm 1 togoal: 9.58128888770675
Mobile Platform 2 Pose: [31.4812477958853;-11.4183222292651;-4.30262832172814]
Robot Arm 2 end: [-11.3398919656375;-3.83195533493538;0.483468207640394]
Robot Arm 1 togoal: 6.39660516914785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.161144e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.270504e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-36.1252305567174;12.3543898646038;1.07945415271955]
Robot Arm 1 end: [12.2855009755797;0.703739840165168;0.275702799292093]
Robot Arm 1 togoal: 9.65374190595539
Mobile Platform 2 Pose: [31.0448827670698;-11.1681611963458;-4.33328465440567]
Robot Arm 2 end: [-11.3301256561338;-4.14070816428588;0.314248638323435]
Robot Arm 1 togoal: 6.1283589912863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.052820e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.775988e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-36.0778235450353;12.1934728718156;0.973923671086511]
Robot Arm 1 end: [12.1572529623037;0.650992866004622;0.155755072281131]
Robot Arm 1 togoal: 9.69131407227765
Mobile Platform 2 Pose: [30.9289367668297;-11.0978430408753;-4.30300576250836]
Robot Arm 2 end: [-11.6353560366324;-3.96646020341429;0.417757632922397]
Robot Arm 1 togoal: 6.34784495431705

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.074718e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.535736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-35.9222974733998;12.0041879821868;0.916658743912407]
Robot Arm 1 end: [12.0482713680942;0.627147187646193;0.165179534509324]
Robot Arm 1 togoal: 9.68937740900036
Mobile Platform 2 Pose: [31.3312589536944;-11.3070761389233;-4.39884782399521]
Robot Arm 2 end: [-11.4588648212414;-4.64837112381734;0.26844171813648]
Robot Arm 1 togoal: 5.6865736999104

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.679171e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.091769e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-35.9525824178066;11.9849099962421;0.854632147917077]
Robot Arm 1 end: [11.9438039624309;0.625766090047491;0.158010219401623]
Robot Arm 1 togoal: 9.67017974935736
Mobile Platform 2 Pose: [31.0514410392285;-11.1373100446284;-4.34386712898867]
Robot Arm 2 end: [-11.3711414101896;-4.53950576856848;0.220010014139977]
Robot Arm 1 togoal: 5.7783735567668

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.090916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.441543e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-35.8756396445242;11.8924520023459;0.766294966724709]
Robot Arm 1 end: [11.7856924915922;0.67095025367104;0.202957185307469]
Robot Arm 1 togoal: 9.58942666197369
Mobile Platform 2 Pose: [30.3754567021855;-10.6485597866265;-4.37075784079516]
Robot Arm 2 end: [-10.781050825009;-4.76106250313992;0.795504782249162]
Robot Arm 1 togoal: 5.34629308944367

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.040345e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.281867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-35.5339320113419;11.6421955812495;0.606574235083113]
Robot Arm 1 end: [11.187223525205;0.793368916816189;0.432532317138117]
Robot Arm 1 togoal: 9.34646015865264
Mobile Platform 2 Pose: [30.4480899003544;-10.6312854439032;-4.43132656834761]
Robot Arm 2 end: [-10.5985899085801;-4.84293045762538;0.854194026614935]
Robot Arm 1 togoal: 5.23433915119032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.431624e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.992335e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-35.1634639557187;11.3931910646118;0.584275277368279]
Robot Arm 1 end: [10.8551534740684;0.57052151079362;0.468723699965607]
Robot Arm 1 togoal: 9.52647035655028
Mobile Platform 2 Pose: [30.6493666860699;-10.7603794839791;-4.51145950652747]
Robot Arm 2 end: [-10.4298759344503;-4.51380337071181;0.0126119629729907]
Robot Arm 1 togoal: 5.70599520193269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.711067e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.327320e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-34.8308377167194;11.149828826334;0.526971798307201]
Robot Arm 1 end: [10.6290774155375;0.266856461492467;0.553264256628726]
Robot Arm 1 togoal: 9.80134347959138
Mobile Platform 2 Pose: [30.1400110114503;-10.2373103223335;-4.44273429420365]
Robot Arm 2 end: [-9.76858892273011;-4.64896311711523;0.502965850309706]
Robot Arm 1 togoal: 5.45192996457328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.092074e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.667585e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-34.5992863102802;10.9246865713318;0.416086473146339]
Robot Arm 1 end: [10.3824555500696;0.126612771886247;0.637057738979425]
Robot Arm 1 togoal: 9.92025208978466
Mobile Platform 2 Pose: [29.8489841325845;-10.0904569295829;-4.52913841224422]
Robot Arm 2 end: [-10.3456617510066;-4.63148246022587;-0.00590286894385134]
Robot Arm 1 togoal: 5.59212794847903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.069419e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.052531e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-34.4148603553001;10.7235893293711;0.339418388236722]
Robot Arm 1 end: [10.2747746426518;-0.131481395391433;0.817938486943699]
Robot Arm 1 togoal: 10.1595625821463
Mobile Platform 2 Pose: [29.5377906650699;-9.81616034718894;-4.65416634893767]
Robot Arm 2 end: [-9.75974535075078;-4.62086174190364;0.591542531916097]
Robot Arm 1 togoal: 5.46413230816922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.183647e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.483694e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-34.4917183332892;10.7060068661;0.329603940467891]
Robot Arm 1 end: [10.1928171278775;-0.121396451865638;0.658660359246386]
Robot Arm 1 togoal: 10.1598953851236
Mobile Platform 2 Pose: [24.176671378462;-6.14327101073162;-4.35571782215108]
Robot Arm 2 end: [-6.41033966635472;-4.74793928764414;0.0900181005507152]
Robot Arm 1 togoal: 6.52054539388921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.517422e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.357423e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-34.762937470775;10.7081567409557;0.346334505326628]
Robot Arm 1 end: [10.1577664890812;0.00651635974027504;0.586749975076528]
Robot Arm 1 togoal: 10.0382980949106
Mobile Platform 2 Pose: [24.1082529727536;-6.13634523352538;-4.3235096242951]
Robot Arm 2 end: [-6.63790345229678;-4.4077351896309;0.141514697431519]
Robot Arm 1 togoal: 6.66934019265571

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.214666e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.330873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-35.0260745619754;10.7272217988069;0.397369810901778]
Robot Arm 1 end: [10.1728282388858;0.149461995650044;0.694424876847223]
Robot Arm 1 togoal: 9.88666753813003
Mobile Platform 2 Pose: [24.400844096319;-6.29313224799546;-4.36362374886742]
Robot Arm 2 end: [-6.59164901963865;-4.52983807358601;0.859798429906245]
Robot Arm 1 togoal: 6.4789440052229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.402086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.825261e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-35.1022481983044;10.7329170594112;0.417449807630243]
Robot Arm 1 end: [10.0396823083909;0.085926109360152;0.616408004074716]
Robot Arm 1 togoal: 9.95533638174814
Mobile Platform 2 Pose: [24.0252593994323;-6.05369188215794;-4.33593925688454]
Robot Arm 2 end: [-6.11906606719169;-4.35441618556964;1.13870456350915]
Robot Arm 1 togoal: 6.86151693121446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.543504e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.023425e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-34.9868752399562;10.6545107610998;0.380820177664539]
Robot Arm 1 end: [10.1052414420256;-0.0842338013680646;0.549899731959705]
Robot Arm 1 togoal: 10.1314304445306
Mobile Platform 2 Pose: [23.9616605657254;-6.01145589547281;-4.32993098151904]
Robot Arm 2 end: [-6.36147091472055;-4.24447519820405;1.1515636284694]
Robot Arm 1 togoal: 6.8191966448492

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.078227e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-34.9663812994233;10.6381156105225;0.0334149686601311]
Robot Arm 1 end: [10.0324146725308;-0.361971869693084;0.620706966356428]
Robot Arm 1 togoal: 10.4010595270738
Mobile Platform 2 Pose: [23.8869739017576;-5.96579213686923;-4.32737407030081]
Robot Arm 2 end: [-6.32496345685419;-4.25244035763378;1.22413715492952]
Robot Arm 1 togoal: 6.82850371492887

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.096081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.915580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-34.8667441460193;10.4398719774324;-0.130213904355143]
Robot Arm 1 end: [9.94850829870261;-0.611517699813099;0.418844612948608]
Robot Arm 1 togoal: 10.6687255933385
Mobile Platform 2 Pose: [23.9209779168537;-5.97869435219177;-4.34746183177504]
Robot Arm 2 end: [-6.02493880649492;-4.92439103928474;1.01674279622389]
Robot Arm 1 togoal: 6.46662146269311

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.993267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.702417e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-34.7853406095995;10.58111010057;0.0413995948906342]
Robot Arm 1 end: [10.0483937269333;-0.354030093569974;0.39822564438759]
Robot Arm 1 togoal: 10.4148405357442
Mobile Platform 2 Pose: [24.6459427065132;-6.2019573874401;-4.5906015103435]
Robot Arm 2 end: [-5.86491241068035;-4.61082556899796;1.30335947697557]
Robot Arm 1 togoal: 6.79628706106312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.317960e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.062558e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-34.4784954829898;10.4772394179531;-0.220100257906809]
Robot Arm 1 end: [9.88993798234266;-0.584464190017389;0.575283030118324]
Robot Arm 1 togoal: 10.6271998393092
Mobile Platform 2 Pose: [24.6569022775699;-6.20868209431995;-4.58871582427935]
Robot Arm 2 end: [-6.36177451932109;-4.64708456621795;1.37049429928946]
Robot Arm 1 togoal: 6.47402813211603

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.291720e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002197e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-34.5958442160577;10.2379096099509;-0.272748932600349]
Robot Arm 1 end: [9.79518670004193;-0.783370244487046;0.727558727297594]
Robot Arm 1 togoal: 10.8144612150008
Mobile Platform 2 Pose: [24.7776439579502;-6.28634058106152;-4.60116877214101]
Robot Arm 2 end: [-6.92810170499232;-4.5800454706485;1.08981756514614]
Robot Arm 1 togoal: 6.24486865572331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.826342e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.795909e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-34.6965831824889;10.2189451100204;-0.313705215441039]
Robot Arm 1 end: [10.497957417206;-1.03699796862708;0.760626904413525]
Robot Arm 1 togoal: 11.0743601618026
Mobile Platform 2 Pose: [25.01091262833;-6.42829446007291;-4.60297915569626]
Robot Arm 2 end: [-7.16261254182535;-4.77110818955612;0.608463889113513]
Robot Arm 1 togoal: 6.01870412188585

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.448758e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-34.6325481214412;10.1807353290813;-0.305447564507393]
Robot Arm 1 end: [10.2244946554206;-1.03700986955772;0.925158945514615]
Robot Arm 1 togoal: 11.0553612004872
Mobile Platform 2 Pose: [25.9915749002939;-6.99443450367594;-4.6117009961973]
Robot Arm 2 end: [-7.31168692557483;-4.67093196525426;0.00991582746359082]
Robot Arm 1 togoal: 6.15705844389707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.012292e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-34.6598818508545;10.1839612584128;-0.309164411247807]
Robot Arm 1 end: [10.0437088491553;-1.01389074355065;1.00196932487659]
Robot Arm 1 togoal: 11.026200280064
Mobile Platform 2 Pose: [26.076530662509;-7.04695340323768;-4.61737101367906]
Robot Arm 2 end: [-7.52576818098628;-4.52051173302003;0.289045750211986]
Robot Arm 1 togoal: 6.13712685511403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.968959e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.527520e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-34.6522794260933;10.1525668841408;-0.29126496692164]
Robot Arm 1 end: [10.4386179749389;-0.830108537182411;0.225169485247421]
Robot Arm 1 togoal: 10.9161721028048
Mobile Platform 2 Pose: [26.1250658563894;-7.06467771347295;-4.64210984524078]
Robot Arm 2 end: [-7.30023565042158;-3.98208453866389;0.759790094104397]
Robot Arm 1 togoal: 6.63953873125809

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005401e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.627647e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-34.6196457733743;10.1121676195534;-0.295487489037195]
Robot Arm 1 end: [10.1972240490151;-0.733554486991367;0.0751573062125702]
Robot Arm 1 togoal: 10.8322920172003
Mobile Platform 2 Pose: [26.3539557440894;-7.19575225701309;-4.6331911546491]
Robot Arm 2 end: [-6.92990237904499;-3.96074931400497;0.572320304220642]
Robot Arm 1 togoal: 6.84090939975593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004389e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.079123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-34.7045737936645;10.1558356558256;-0.318865509140808]
Robot Arm 1 end: [10.1665622398961;-0.809214256006466;0.113654092726032]
Robot Arm 1 togoal: 10.9017190531922
Mobile Platform 2 Pose: [26.4249620414249;-7.21149599565849;-4.58503914111739]
Robot Arm 2 end: [-6.71136821729113;-4.59645579324309;0.143613574456848]
Robot Arm 1 togoal: 6.47383830211179

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.832445e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.972602e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-34.6822419708603;10.2139299238629;-0.287024492947642]
Robot Arm 1 end: [10.1520179378246;-0.885265809909909;0.255623985172395]
Robot Arm 1 togoal: 10.9596212372835
Mobile Platform 2 Pose: [26.5398009493302;-7.26087102294119;-4.55684304475112]
Robot Arm 2 end: [-6.72162077257914;-4.56943099622579;0.265601892388694]
Robot Arm 1 togoal: 6.46644218037491

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.345200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.087762e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-34.7002035164985;10.2512409843267;-0.295390518450719]
Robot Arm 1 end: [10.0943766333394;-0.436022989563764;1.11326239601138]
Robot Arm 1 togoal: 10.444411555532
Mobile Platform 2 Pose: [26.5572404132595;-7.29895909636683;-4.56881376462474]
Robot Arm 2 end: [-6.6494523990626;-4.5876854057569;0.419257932747363]
Robot Arm 1 togoal: 6.46012028339707

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.022515e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.243854e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-34.6473102285183;10.2105083994774;-0.306355143315879]
Robot Arm 1 end: [9.94967151552936;-0.503919332040732;1.09352447221891]
Robot Arm 1 togoal: 10.5127346402932
Mobile Platform 2 Pose: [25.7782460620935;-6.96436320298802;-4.72101968089269]
Robot Arm 2 end: [-6.84970571668207;-4.29685248555752;0.880327742188947]
Robot Arm 1 togoal: 6.54680888562708

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002472e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.675580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-34.6259802004783;10.1875403550065;-0.302418655298443]
Robot Arm 1 end: [9.70097707762726;-0.382294592096567;1.090577920335]
Robot Arm 1 togoal: 10.3955143630243
Mobile Platform 2 Pose: [25.2446456853699;-6.67264242835553;-4.77777965410519]
Robot Arm 2 end: [-6.21110504210486;-4.48261201377978;0.592801172307693]
Robot Arm 1 togoal: 6.75713313885259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.507883e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.105083e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-34.5504956340476;10.0624403406527;-0.282772857836986]
Robot Arm 1 end: [9.6336251012883;-0.505027646600505;1.06030870922955]
Robot Arm 1 togoal: 10.5215052576919
Mobile Platform 2 Pose: [25.3839368319668;-6.74499159123474;-4.75754420652002]
Robot Arm 2 end: [-6.36068039400447;-4.91855546363635;0.551259940323737]
Robot Arm 1 togoal: 6.32503925316392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.012677e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.853797e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-34.5724795947147;9.98562083115393;-0.245613796006844]
Robot Arm 1 end: [9.65383673023966;-0.603177539590633;1.01642601546417]
Robot Arm 1 togoal: 10.6208190762898
Mobile Platform 2 Pose: [25.4125792608517;-6.7709483257068;-4.75621575170456]
Robot Arm 2 end: [-6.43751587274398;-4.94005293184753;0.691987512846374]
Robot Arm 1 togoal: 6.2435261826401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.256839e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.145812e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-34.6075873642016;9.94725531749915;-0.187417757957145]
Robot Arm 1 end: [9.9785539759919;-0.520170465609681;0.584278916229303]
Robot Arm 1 togoal: 10.5618129573579
Mobile Platform 2 Pose: [25.4096582799119;-6.77385042408242;-4.76114445413235]
Robot Arm 2 end: [-6.40573215732968;-4.88751739215205;0.850955424683229]
Robot Arm 1 togoal: 6.28531617926363

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.020156e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-33.9801781627153;9.28746775209322;-0.187646270362732]
Robot Arm 1 end: [9.32505835511163;-0.524436065218587;0.233773548500324]
Robot Arm 1 togoal: 10.6243236326986
Mobile Platform 2 Pose: [25.3889893557666;-6.76009530848352;-4.76502772348748]
Robot Arm 2 end: [-6.47239388505781;-4.85733784876571;0.796687105278315]
Robot Arm 1 togoal: 6.27818509390853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.542389e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.096894e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-34.0314975240689;9.24865586397219;-0.279672279302311]
Robot Arm 1 end: [9.18908355439603;-0.480638872908765;0.020611976334015]
Robot Arm 1 togoal: 10.6184999358496
Mobile Platform 2 Pose: [25.2993158573973;-6.69825647727696;-4.77902797415892]
Robot Arm 2 end: [-6.5729106730181;-4.8220756013596;0.718948130673415]
Robot Arm 1 togoal: 6.26092082159158

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.295972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.171581e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-33.9948174846119;9.22123660882207;-0.270036125338734]
Robot Arm 1 end: [9.54751751042398;-0.791338499442223;0.666978076264373]
Robot Arm 1 togoal: 10.8345318523293
Mobile Platform 2 Pose: [25.1477946348651;-6.59456781779506;-4.80279153594449]
Robot Arm 2 end: [-6.69644414321796;-4.8396770996336;0.673593229363734]
Robot Arm 1 togoal: 6.18550822243775

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.707829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.346566e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-33.8807669450164;9.1568601242552;-0.255049601467344]
Robot Arm 1 end: [9.06061199853678;-0.043774191683799;0.0793932862316236]
Robot Arm 1 togoal: 10.190097141245
Mobile Platform 2 Pose: [25.3498456875994;-6.72441822123166;-4.83510722949254]
Robot Arm 2 end: [-6.82693629691517;-5.00105308923898;0.789296940724871]
Robot Arm 1 togoal: 5.9660030881147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.068892e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.059222e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-33.7936269580826;9.08660778783176;-0.210893301701635]
Robot Arm 1 end: [9.08235079670314;-0.130861104274034;0.0580194178409433]
Robot Arm 1 togoal: 10.2770004844886
Mobile Platform 2 Pose: [24.9056452067004;-6.34938629871416;-4.89396345878823]
Robot Arm 2 end: [-6.77645584470486;-4.8793749268112;0.981284948358559]
Robot Arm 1 togoal: 6.07481114095538

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.907282e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.947193e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-33.4071940261473;8.83752613158504;-0.2323138905236]
Robot Arm 1 end: [8.85651954526789;-0.206826314437074;-0.0114538171686017]
Robot Arm 1 togoal: 10.3843760363921
Mobile Platform 2 Pose: [25.0409061812489;-6.42026674724238;-4.91998851313138]
Robot Arm 2 end: [-6.75127731037309;-4.48087043865723;1.12215105118657]
Robot Arm 1 togoal: 6.41670247969733

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.876419e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.116503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-33.1025039050961;8.63382512006096;-0.215029816652841]
Robot Arm 1 end: [8.90928385729925;-0.205846900775763;-0.121608078482649]
Robot Arm 1 togoal: 10.3945723320163
Mobile Platform 2 Pose: [25.122160244375;-6.46710697085486;-4.92756423472885]
Robot Arm 2 end: [-6.62237996995312;-4.28884836053196;0.889996550772556]
Robot Arm 1 togoal: 6.66511331256336

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.350252e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.872646e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-32.7935554894745;8.44629615392067;-0.188525911873436]
Robot Arm 1 end: [7.59723428598646;-0.248664405193748;0.514465188471999]
Robot Arm 1 togoal: 10.5745693762698
Mobile Platform 2 Pose: [25.298285730732;-6.56848221867338;-4.92282471922264]
Robot Arm 2 end: [-6.43295136953556;-4.29008023230857;0.501324050716366]
Robot Arm 1 togoal: 6.80931411576225

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.890607e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.216851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-33.040182052576;8.55653946447062;-0.156140424350193]
Robot Arm 1 end: [7.78712234721385;-0.00222524735876939;0.346508949824494]
Robot Arm 1 togoal: 10.3111961785257
Mobile Platform 2 Pose: [25.6098537798969;-6.72139687925904;-4.84405357476081]
Robot Arm 2 end: [-6.32301826984872;-4.40800482928948;0.275492754282619]
Robot Arm 1 togoal: 6.80748800456164

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.375386e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.663774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-33.1565771390398;8.61745465027666;-0.142550134300967]
Robot Arm 1 end: [8.03836448836772;0.172043816348946;0.181417653041509]
Robot Arm 1 togoal: 10.1109454302031
Mobile Platform 2 Pose: [25.8178321471956;-6.80924021374913;-4.78862471647955]
Robot Arm 2 end: [-6.45096954984184;-4.76502074063639;0.0422518024085918]
Robot Arm 1 togoal: 6.49517676541577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.840829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.703502e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [-33.2973954218478;8.68915749525897;-0.124135611491522]
Robot Arm 1 end: [8.35315575767789;0.225244704415643;0.0522767737381865]
Robot Arm 1 togoal: 10.0207327557612
Mobile Platform 2 Pose: [25.9340803419318;-6.86876599283433;-4.75872463585563]
Robot Arm 2 end: [-6.60634899804769;-4.84862957662961;0.0132231984791788]
Robot Arm 1 togoal: 6.350344498138

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.468535e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.736585e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [-33.3731855521032;8.72713590926146;-0.109852666229764]
Robot Arm 1 end: [8.59424502350398;0.148974252246716;-0.0243130914093265]
Robot Arm 1 togoal: 10.0700967168803
Mobile Platform 2 Pose: [25.9931435655984;-6.90552073435693;-4.74287550720968]
Robot Arm 2 end: [-6.61563947882651;-4.82740021626051;0.0154135304053482]
Robot Arm 1 togoal: 6.36211248544429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.350023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.760106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [-33.3253579791963;8.69161259078553;-0.0923802668688146]
Robot Arm 1 end: [8.70092101688574;0.0117717111847471;-0.0360011531544784]
Robot Arm 1 togoal: 10.1919852406086
Mobile Platform 2 Pose: [25.8492621251187;-6.83437437814491;-4.80186600545839]
Robot Arm 2 end: [-6.283351959004;-4.66520610512182;0.202452336582709]
Robot Arm 1 togoal: 6.63415906518468

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.024822e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.583459e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [-33.1644017881434;8.58488251630495;-0.0709274497275545]
Robot Arm 1 end: [8.71358619353384;-0.0907333694112498;-0.0167895902716809]
Robot Arm 1 togoal: 10.2879812032276
Mobile Platform 2 Pose: [25.2600064739117;-6.71652324789773;-4.97864270089875]
Robot Arm 2 end: [-6.26842148282109;-4.7199806792219;0.133766555318001]
Robot Arm 1 togoal: 6.612692257145

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.976420e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.151370e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [-33.0481277764131;8.51244559020971;-0.0493645955286187]
Robot Arm 1 end: [8.7163754179826;-0.0263388383797425;0.000620090266717666]
Robot Arm 1 togoal: 10.2218744679967
Mobile Platform 2 Pose: [25.7510065732878;-7.01608316895189;-4.95438681379964]
Robot Arm 2 end: [-6.88308791237966;-5.01179299424838;-0.0684868843349134]
Robot Arm 1 togoal: 6.09293185992229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.524716e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.487596e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [-32.9692595458898;8.48620989999379;0.0486817746993165]
Robot Arm 1 end: [8.26725053526842;0.045877424288249;-0.0694834203540343]
Robot Arm 1 togoal: 10.2282263599435
Mobile Platform 2 Pose: [25.4728856294639;-6.78751214517957;-4.85311842242468]
Robot Arm 2 end: [-6.54426866177828;-4.91476303443284;-0.0306655121495321]
Robot Arm 1 togoal: 6.34108665289575

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.984989e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.938621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [-32.3867869497587;8.13144142584993;-0.0855884833601724]
Robot Arm 1 end: [8.09543992490868;-0.171314960227122;-0.116580614104371]
Robot Arm 1 togoal: 10.4768634317463
Mobile Platform 2 Pose: [25.3460752179926;-6.78721499850848;-5.14356375395342]
Robot Arm 2 end: [-7.27805815642784;-4.96024259860727;0.398568938763483]
Robot Arm 1 togoal: 5.83677766853512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.378235e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.495790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [-32.2990241340013;8.07172754044045;-0.0836188004975048]
Robot Arm 1 end: [7.88105846740173;-0.0958683492166667;-0.0565712194926745]
Robot Arm 1 togoal: 10.435765515565
Mobile Platform 2 Pose: [25.233288611591;-6.7095998399706;-5.16666660070133]
Robot Arm 2 end: [-6.75627233926792;-5.09616852671286;0.678864167204225]
Robot Arm 1 togoal: 5.93957279745594

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.866806e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.322787e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [-32.4034590037973;8.26361264751611;-0.297504518913072]
Robot Arm 1 end: [8.19884127993897;-0.207396708793111;-0.0615302448236478]
Robot Arm 1 togoal: 10.4852049231912
Mobile Platform 2 Pose: [25.2331801141212;-6.70855763286426;-5.21326423638142]
Robot Arm 2 end: [-6.73819704350125;-5.19285694032912;0.545310151895359]
Robot Arm 1 togoal: 5.89066665181967

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.678991e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.145955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [-32.8337417418775;8.56910708197502;-0.388427634520947]
Robot Arm 1 end: [8.64899132735651;-0.0948553304210624;0.080094414785941]
Robot Arm 1 togoal: 10.2862790882413
Mobile Platform 2 Pose: [24.4468074828175;-6.19772971576537;-5.23200031305676]
Robot Arm 2 end: [-6.16175826350136;-5.19602858070841;0.48348089528349]
Robot Arm 1 togoal: 6.23591909166743

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.430419e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.802861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [-32.4802340924649;8.34084063181064;-0.292401330367936]
Robot Arm 1 end: [8.66199134879406;-0.100128954019067;0.0636304550063506]
Robot Arm 1 togoal: 10.2920745250504
Mobile Platform 2 Pose: [23.5458346300735;-5.6639817546233;-5.14873877830903]
Robot Arm 2 end: [-6.2003104588414;-4.64267630885626;0.417286830593643]
Robot Arm 1 togoal: 6.6600856226629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.489719e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.142465e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-32.3195229970038;8.26378854650351;-0.226213718569488]
Robot Arm 1 end: [8.75307206955781;-0.239845322875886;0.089595679417589]
Robot Arm 1 togoal: 10.4143257020857
Mobile Platform 2 Pose: [23.5767387405261;-5.67542282287657;-5.17466883191306]
Robot Arm 2 end: [-6.14497170222097;-4.89170649244416;1.04559686911051]
Robot Arm 1 togoal: 6.41731360250329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.466027e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.775356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-32.305880367131;8.26978982622419;-0.210878966871524]
Robot Arm 1 end: [8.81737558290643;-0.382959505938639;0.237316400576527]
Robot Arm 1 togoal: 10.5286415171805
Mobile Platform 2 Pose: [23.5359820870572;-5.64149956642072;-5.20355192194383]
Robot Arm 2 end: [-5.34852156700765;-5.10793405988609;1.12384671322226]
Robot Arm 1 togoal: 6.76212181997736

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.842007e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.637581e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-32.4466603800708;8.35375467408294;-0.223287452097133]
Robot Arm 1 end: [8.89202481799862;-0.546996715695078;0.313699406364655]
Robot Arm 1 togoal: 10.6735337844692
Mobile Platform 2 Pose: [23.45819116686;-5.67261222098282;-5.33377849732199]
Robot Arm 2 end: [-5.08715544203532;-5.43274231574054;0.758297034535347]
Robot Arm 1 togoal: 6.75111844205675

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.699503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.400983e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [-32.6556507781205;8.47445893559831;-0.231121300569735]
Robot Arm 1 end: [9.11347701723826;-0.278865381635882;0.581876633369927]
Robot Arm 1 togoal: 10.3596790119438
Mobile Platform 2 Pose: [23.4615772173458;-5.63483313814956;-5.31931965768106]
Robot Arm 2 end: [-5.106109937844;-5.4841930918474;0.925317720189776]
Robot Arm 1 togoal: 6.68561959362541

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.534040e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.636832e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [-32.7757000741784;8.46882853915977;-0.32828528438083]
Robot Arm 1 end: [9.06986890572929;-0.431760738200275;0.564984861825717]
Robot Arm 1 togoal: 10.5166886849428
Mobile Platform 2 Pose: [23.5506637145051;-5.64124489332785;-5.28796321692988]
Robot Arm 2 end: [-5.19952561691155;-5.47694684123204;1.08046074229575]
Robot Arm 1 togoal: 6.61034333581152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.259181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.691350e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [-33.5939653434277;8.83701472944901;-0.436359094103887]
Robot Arm 1 end: [9.4090382640547;-0.554775091942259;0.55070044633745]
Robot Arm 1 togoal: 10.6157427549218
Mobile Platform 2 Pose: [23.408561346602;-5.45287829859925;-5.27593834683029]
Robot Arm 2 end: [-5.30071913772954;-4.991410780993;1.15887893778738]
Robot Arm 1 togoal: 6.87752413670447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.165440e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.585283e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [-34.030970519617;8.987214507214;-0.615356894022465]
Robot Arm 1 end: [9.52086120840219;-0.66984814406845;0.430992490979652]
Robot Arm 1 togoal: 10.7360770195084
Mobile Platform 2 Pose: [22.9303138065058;-5.19208577032171;-5.38602480746964]
Robot Arm 2 end: [-4.65844602819131;-5.64250226133286;0.848913039503848]
Robot Arm 1 togoal: 6.92615956058647

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.587106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.087926e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [-34.3812655702368;9.09143384181039;-0.80531525771343]
Robot Arm 1 end: [9.56767600797264;-0.830461782628694;0.337352802381966]
Robot Arm 1 togoal: 10.9035236115438
Mobile Platform 2 Pose: [23.0498258476592;-5.21724889312086;-5.35409347263183]
Robot Arm 2 end: [-5.19475539377037;-5.0316726239851;1.15219589068037]
Robot Arm 1 togoal: 6.92175332643465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.415156e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.020558e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [-34.8582606318392;9.27800995311703;-0.999106828967844]
Robot Arm 1 end: [9.16923836968957;-0.886936918128273;0.386829719901785]
Robot Arm 1 togoal: 10.977335859395
Mobile Platform 2 Pose: [23.4520853364079;-5.45626974675562;-5.34340330912572]
Robot Arm 2 end: [-5.69937365216744;-5.04055004978172;0.446549691812613]
Robot Arm 1 togoal: 6.65176474758058

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.952985e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [-33.7393943028996;8.5416287349871;-0.981098632212539]
Robot Arm 1 end: [8.48863884863657;-1.19419874474141;1.3618813190414]
Robot Arm 1 togoal: 11.2968852707837
Mobile Platform 2 Pose: [23.14248315823;-5.26646728749152;-5.35608719083325]
Robot Arm 2 end: [-5.4585547938789;-4.92080594319552;0.57670170617367]
Robot Arm 1 togoal: 6.87856351970392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.911554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.154050e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [-33.4884340838609;8.36300732822463;-0.853153136596486]
Robot Arm 1 end: [8.16638414736529;-0.898242047639483;1.30608021992441]
Robot Arm 1 togoal: 11.0535070152345
Mobile Platform 2 Pose: [22.8446010411703;-5.08930645060804;-5.34542570505251]
Robot Arm 2 end: [-5.18651832091953;-4.93974467430341;0.573367473856387]
Robot Arm 1 togoal: 7.04796404933292

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.844324e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.777944e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [-33.1961164700442;8.0214735009482;-0.673790707123429]
Robot Arm 1 end: [7.84928480456635;-0.645023409829022;1.06898944416789]
Robot Arm 1 togoal: 10.8695175969433
Mobile Platform 2 Pose: [22.9368050335368;-5.14493032008127;-5.34947037305283]
Robot Arm 2 end: [-5.16461100605399;-5.03697914145096;0.462516753922804]
Robot Arm 1 togoal: 7.00948996342767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.770585e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.849626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [-33.1495596759041;7.96876977364354;-0.631286180940875]
Robot Arm 1 end: [7.49910794345301;-0.459658361240842;0.633369929423617]
Robot Arm 1 togoal: 10.7910519067683
Mobile Platform 2 Pose: [23.1873018211447;-5.30140600487176;-5.37041729973782]
Robot Arm 2 end: [-5.28918986744602;-5.15862841388925;0.379779751250093]
Robot Arm 1 togoal: 6.8507659860727

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.748783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.145058e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [-32.962742716439;7.86713893575762;-0.673068772490906]
Robot Arm 1 end: [7.68506932047543;-0.464736576120647;0.612681737437196]
Robot Arm 1 togoal: 10.7561451145244
Mobile Platform 2 Pose: [23.4149914015761;-5.44691739455471;-5.38472537628942]
Robot Arm 2 end: [-5.46675699156084;-5.43496415942109;0.516053724571812]
Robot Arm 1 togoal: 6.51150991827775

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.038782e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.365024e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [-33.0290786392647;7.8891559660288;-0.648978894160918]
Robot Arm 1 end: [7.72962614411433;-0.510793014180577;0.664805273219373]
Robot Arm 1 togoal: 10.787234893271
Mobile Platform 2 Pose: [23.4512709076373;-5.47290729335775;-5.37529644498356]
Robot Arm 2 end: [-5.43153607603083;-5.34263981017868;0.618132525390144]
Robot Arm 1 togoal: 6.58612452350005

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.254840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.908478e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [-33.0182671513642;7.8498474079318;-0.577192915030825]
Robot Arm 1 end: [7.7176945248284;-0.538974901503945;0.63218629118231]
Robot Arm 1 togoal: 10.8198382642954
Mobile Platform 2 Pose: [23.8018120676924;-5.69404422250607;-5.24958340847189]
Robot Arm 2 end: [-5.69673639518273;-5.25805667137542;0.626376756284481]
Robot Arm 1 togoal: 6.46563646771361

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.800506e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.163943e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [-32.6096987217948;7.53441468112365;-0.540331978618987]
Robot Arm 1 end: [7.46056972165745;-0.972656490134652;0.5089609583271]
Robot Arm 1 togoal: 11.3080555185641
Mobile Platform 2 Pose: [23.6976181723755;-5.62341448046435;-5.20348572056567]
Robot Arm 2 end: [-5.67787455870287;-5.25139972877327;0.585013184765815]
Robot Arm 1 togoal: 6.48891702709279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.338695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.411222e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [-32.5844875086247;7.4697449214359;-0.551286441362565]
Robot Arm 1 end: [7.70570420309694;-0.344875822116211;0.181673649124376]
Robot Arm 1 togoal: 10.6805451196482
Mobile Platform 2 Pose: [23.4892029871247;-5.4871963045059;-5.15352357472265]
Robot Arm 2 end: [-5.55257222958584;-5.19309581979716;0.596303411385791]
Robot Arm 1 togoal: 6.61369831120965

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.121580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.056524e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [-31.5849234411059;6.89209645305902;-0.629152875980049]
Robot Arm 1 end: [6.37275777617053;-0.930817203947317;0.751786079335206]
Robot Arm 1 togoal: 11.5425881392427
Mobile Platform 2 Pose: [23.4443777981428;-5.46817233132231;-5.0783710903286]
Robot Arm 2 end: [-5.52326559976814;-5.09894999593149;0.581722599359335]
Robot Arm 1 togoal: 6.70400507666525

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.958447e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.137550e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [-31.4568387067042;6.78665994536992;-0.677301744094951]
Robot Arm 1 end: [6.17429533177255;-0.80452419652283;0.616332642803128]
Robot Arm 1 togoal: 11.4974859142737
Mobile Platform 2 Pose: [22.6961048472486;-4.86955397060697;-4.85388920506035]
Robot Arm 2 end: [-4.96549781872421;-4.871089880385;0.62153959299826]
Robot Arm 1 togoal: 7.24299387353901

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.062690e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.467233e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [-31.2088909652242;6.57619774662449;-0.711749785126612]
Robot Arm 1 end: [5.98505225346647;-0.883056367506519;0.449567235701499]
Robot Arm 1 togoal: 11.6494072584888
Mobile Platform 2 Pose: [22.3827152363692;-4.70714861827107;-4.59435269773088]
Robot Arm 2 end: [-4.73984287304461;-4.65836809831454;0.607044203318694]
Robot Arm 1 togoal: 7.55232411722714

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.698986e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.371467e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [-31.0662576942973;6.37765468810303;-0.728302599375017]
Robot Arm 1 end: [6.62475572199294;-1.37214636548808;0.361055564792406]
Robot Arm 1 togoal: 11.919037628394
Mobile Platform 2 Pose: [22.3827152428161;-4.7071486229698;-4.2407993046262]
Robot Arm 2 end: [-4.77048135018309;-4.27836733354956;0.596213042431227]
Robot Arm 1 togoal: 7.80641893543159

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.023551e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.870220e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [-31.2370273176805;6.46266783895275;-0.720333866469978]
Robot Arm 1 end: [6.13729816296235;-1.36730369701075;0.367856745004471]
Robot Arm 1 togoal: 12.0609202877919
Mobile Platform 2 Pose: [24.4650851030591;-6.25273289978206;-3.72704916189177]
Robot Arm 2 end: [-6.24438153326033;-3.95769447962581;0.809106049035427]
Robot Arm 1 togoal: 7.14988944436769

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.377761e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [-31.3148782860415;6.45859895720518;-0.676695056148627]
Robot Arm 1 end: [6.51278609810566;-0.409612868302152;-0.0316855667950937]
Robot Arm 1 togoal: 11.0874493610939
Mobile Platform 2 Pose: [24.5531761553084;-6.27432162278988;-3.8959754561888]
Robot Arm 2 end: [-5.85289049707894;-3.79253101221553;1.18031369699492]
Robot Arm 1 togoal: 7.47310214171996

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.926248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.747967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [-31.5558059451827;6.57987497756695;-0.45549919924062]
Robot Arm 1 end: [6.71382145236954;-1.01931606768972;0.982547732901117]
Robot Arm 1 togoal: 11.511482393267
Mobile Platform 2 Pose: [24.5722295547305;-6.24252873286249;-3.84601728904317]
Robot Arm 2 end: [-5.7539429414262;-4.00381965351258;0.43830199528539]
Robot Arm 1 togoal: 7.4266691229433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.051627e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.868118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [-31.4820300319477;6.54700528249819;-0.438392519826687]
Robot Arm 1 end: [6.63930933895951;-1.09355994663666;0.84794394918069]
Robot Arm 1 togoal: 11.6109568277714
Mobile Platform 2 Pose: [24.4785738030601;-6.17927633394647;-3.85117946010891]
Robot Arm 2 end: [-5.63622171764134;-4.09623224351113;0.457320467395832]
Robot Arm 1 togoal: 7.41811625469861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008403e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.256774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [-31.4474856519121;6.54624568569278;-0.431418345691879]
Robot Arm 1 end: [6.75242418544931;-0.885218054777308;0.102714111811551]
Robot Arm 1 togoal: 11.4475436505802
Mobile Platform 2 Pose: [24.3974825404286;-6.12608015077915;-3.85454235335128]
Robot Arm 2 end: [-5.95243988454165;-3.86710337729594;0.144424881370991]
Robot Arm 1 togoal: 7.47596969832359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.366155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [-31.0300961373518;6.39572865474451;-0.309308247922147]
Robot Arm 1 end: [5.93699964709579;-0.00518170891153186;0.878434774517032]
Robot Arm 1 togoal: 10.8177873414593
Mobile Platform 2 Pose: [24.3029175412907;-6.05533352349165;-3.80374496671407]
Robot Arm 2 end: [-5.51616878104599;-3.54588753382065;0.615942230234416]
Robot Arm 1 togoal: 7.91071676226443

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.101498e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.878611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [-30.8688448127768;6.27436625467861;-0.296870550445769]
Robot Arm 1 end: [5.81952447863184;-0.0290164475303198;1.18165940021316]
Robot Arm 1 togoal: 10.8707266791257
Mobile Platform 2 Pose: [23.6756422678311;-5.75080748840409;-3.88853055131488]
Robot Arm 2 end: [-5.2979254347004;-3.92465068348849;0.11649546734615]
Robot Arm 1 togoal: 7.80973799288552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.073276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.216673e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [-30.6957379980908;6.17322899601737;-0.312667199649081]
Robot Arm 1 end: [5.59383460134628;-0.158134225567282;1.12800806233875]
Robot Arm 1 togoal: 11.0795499515098
Mobile Platform 2 Pose: [23.181037868156;-5.47246950714748;-3.98124165034341]
Robot Arm 2 end: [-5.54402829445966;-4.54283065609175;0.0292284135950507]
Robot Arm 1 togoal: 7.20151119937151

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.993525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.345206e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [-30.4877917924365;6.06269186864237;-0.341262419780705]
Robot Arm 1 end: [5.44173447892477;-0.382898663043408;1.07426948801843]
Robot Arm 1 togoal: 11.3482129587873
Mobile Platform 2 Pose: [23.120033694669;-5.45917631350697;-3.96732851138924]
Robot Arm 2 end: [-5.43502522465283;-4.69000182351357;0.198809733967921]
Robot Arm 1 togoal: 7.12623760714131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.882457e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.882045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [-30.2841150909797;5.96536035938674;-0.378234367056463]
Robot Arm 1 end: [5.4031849535393;-0.474849161894085;1.13116230001033]
Robot Arm 1 togoal: 11.445747986398
Mobile Platform 2 Pose: [23.019993038693;-5.43713579532674;-3.95570177925804]
Robot Arm 2 end: [-5.45660924345593;-4.80882918334503;0.668481679042451]
Robot Arm 1 togoal: 6.95107484508033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.263900e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.265924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [-30.1331018825735;5.89795640269226;-0.411823071160391]
Robot Arm 1 end: [5.39969327485752;-0.303182240797546;1.22248183526365]
Robot Arm 1 togoal: 11.2874930497057
Mobile Platform 2 Pose: [22.8673991040436;-5.3933714168929;-3.94511309759258]
Robot Arm 2 end: [-5.51252482414183;-4.57797211568868;1.19948745813762]
Robot Arm 1 togoal: 7.04550850874283

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.078255e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.279056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [-29.800802219818;5.72530755117875;-0.468824705026498]
Robot Arm 1 end: [5.51578545875321;-0.0270843376982858;1.17663043548986]
Robot Arm 1 togoal: 10.9895036632327
Mobile Platform 2 Pose: [22.7903997530929;-5.40161898314826;-3.93348639332115]
Robot Arm 2 end: [-5.53571593170053;-4.28820995037528;1.41584063312067]
Robot Arm 1 togoal: 7.2502025010276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024358e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.074301e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [-29.7679566662421;5.71138666687257;-0.473095479450947]
Robot Arm 1 end: [5.65536518476933;-0.131461809525017;1.28652799333032]
Robot Arm 1 togoal: 11.0262118244432
Mobile Platform 2 Pose: [22.8799795663591;-5.48961189601211;-3.92948224500945]
Robot Arm 2 end: [-5.47826342310243;-3.66769116469203;1.47118563492753]
Robot Arm 1 togoal: 7.78117718134861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024126e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.882494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [-29.8129246997207;5.73891517820014;-0.475232009501442]
Robot Arm 1 end: [5.76014349213422;-0.282712429709805;1.36751573661933]
Robot Arm 1 togoal: 11.1235837542811
Mobile Platform 2 Pose: [23.1496237469104;-5.67985064826418;-4.02526628137558]
Robot Arm 2 end: [-5.82870898617642;-3.41941032035021;1.15306234851382]
Robot Arm 1 togoal: 7.79994919017425

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.026973e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.016201e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [-29.8573295024811;5.76626132974614;-0.477236457879385]
Robot Arm 1 end: [5.83522922006261;-0.477461674543983;1.40790740262413]
Robot Arm 1 togoal: 11.2754294253718
Mobile Platform 2 Pose: [23.3476256265913;-5.80078065308938;-4.03786422888976]
Robot Arm 2 end: [-6.07114610709622;-3.37643481485971;0.71948398932554]
Robot Arm 1 togoal: 7.74273443874676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.023874e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.022215e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [-29.8770901911992;5.77848695200749;-0.476442141298006]
Robot Arm 1 end: [5.87307605939144;-0.626295980451048;1.40077094127972]
Robot Arm 1 togoal: 11.4001808100696
Mobile Platform 2 Pose: [23.6033750472973;-5.95132176504209;-4.04267365720983]
Robot Arm 2 end: [-6.29061210768617;-3.59120482436298;0.370352274404828]
Robot Arm 1 togoal: 7.49374434627511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.014242e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.822264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [-29.8664261656441;5.77073275961764;-0.473722329980093]
Robot Arm 1 end: [5.83439999695597;-0.727397076742111;1.3862658253935]
Robot Arm 1 togoal: 11.5085804826339
Mobile Platform 2 Pose: [23.7880404850126;-6.07200318876312;-4.0330027006792]
Robot Arm 2 end: [-6.36354810000789;-3.86306991989599;0.131737178075141]
Robot Arm 1 togoal: 7.26744414615098

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.814627e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.357571e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [-29.8217823083663;5.7344733760456;-0.473739788500988]
Robot Arm 1 end: [5.71536210266176;-0.769237627929194;1.35926360698987]
Robot Arm 1 togoal: 11.5914088729083
Mobile Platform 2 Pose: [23.5088408203646;-5.92646873646068;-3.99419004482085]
Robot Arm 2 end: [-6.15159235855397;-3.85575351424528;0.104914599567818]
Robot Arm 1 togoal: 7.38696853342199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.390929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.568063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [-29.7986773706085;5.71175683978386;-0.477229908299704]
Robot Arm 1 end: [5.5570849666769;-0.640579799540186;1.26321213237747]
Robot Arm 1 togoal: 11.5337715881842
Mobile Platform 2 Pose: [23.0543384052415;-5.66463990661517;-3.98083888110064]
Robot Arm 2 end: [-5.4852993518816;-3.64711052060541;0.0418784701776114]
Robot Arm 1 togoal: 7.93281332060116

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.495399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [-29.7799656532665;5.69496022755326;-0.472922246669753]
Robot Arm 1 end: [5.68027913740001;-0.183661018795519;1.10253328856657]
Robot Arm 1 togoal: 11.0698714746421
Mobile Platform 2 Pose: [24.5053431569798;-6.47542925997374;-3.91703748913723]
Robot Arm 2 end: [-6.29695528807596;-3.8630747874228;-0.00278041975400539]
Robot Arm 1 togoal: 7.32777578606906

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.272923e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.295831e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [-29.3190377288645;5.40385855296924;-0.414725187198239]
Robot Arm 1 end: [5.93161608374739;0.120070872669954;0.589957108835794]
Robot Arm 1 togoal: 10.7252779971558
Mobile Platform 2 Pose: [24.8606039395677;-6.7251764170835;-3.90579660132351]
Robot Arm 2 end: [-6.47950747681957;-3.93442948011649;0.0421379983877229]
Robot Arm 1 togoal: 7.16742953626067

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.318977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.619217e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [-28.6462214602888;5.04935976755424;-0.314492065465016]
Robot Arm 1 end: [5.4946922293997;0.278967825094198;0.791052623722972]
Robot Arm 1 togoal: 10.7391381323419
Mobile Platform 2 Pose: [24.9561978002926;-6.73816128108603;-3.7659946656546]
Robot Arm 2 end: [-6.44059619481428;-3.86275819884787;0.0419821648086752]
Robot Arm 1 togoal: 7.24724679661136

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.904603e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.640701e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [-28.1683414310171;4.80979665448252;-0.133107234156719]
Robot Arm 1 end: [4.5561784511713;0.238858240204008;1.21438790936965]
Robot Arm 1 togoal: 11.1807465083708
Mobile Platform 2 Pose: [24.9443201667755;-6.68629956726244;-3.69833832337065]
Robot Arm 2 end: [-6.37420607180206;-3.75734103758974;0.0681482505319878]
Robot Arm 1 togoal: 7.3639629776806

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.040891e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.857902e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [-28.068997787014;4.73790207643805;-0.116079615995804]
Robot Arm 1 end: [4.77324579958198;0.586175966612656;0.492154197757266]
Robot Arm 1 togoal: 10.8165413181831
Mobile Platform 2 Pose: [24.9369025687087;-6.69956601687303;-3.68543891789294]
Robot Arm 2 end: [-6.44784919093444;-3.77110329226903;0.133008971329327]
Robot Arm 1 togoal: 7.30365994975103

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.238721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.677637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [-28.7419168176172;5.07878515798548;-0.0616236400155604]
Robot Arm 1 end: [5.04785119080588;0.717923465567627;0.577347253022709]
Robot Arm 1 togoal: 10.5627270686287
Mobile Platform 2 Pose: [24.7435570027914;-6.57341168531678;-3.71520972228219]
Robot Arm 2 end: [-6.08762326024249;-4.01864424814241;0.362727183119142]
Robot Arm 1 togoal: 7.24050442320151

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.087442e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.729210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [-28.9706596030977;5.19020311921799;-0.00182382542678941]
Robot Arm 1 end: [5.37342435421456;0.701838847928768;1.08781569251347]
Robot Arm 1 togoal: 10.3946453359561
Mobile Platform 2 Pose: [24.6534898692885;-6.4460847945947;-3.73529181239319]
Robot Arm 2 end: [-6.4915804026898;-3.30613654805301;0.169689234353029]
Robot Arm 1 togoal: 7.67742513929347

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.961468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.090916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [-28.8833971039188;5.1283167595717;0.076455819103506]
Robot Arm 1 end: [5.08161334233353;-0.028921631541478;1.51108433945537]
Robot Arm 1 togoal: 11.1700445266004
Mobile Platform 2 Pose: [24.4614899321991;-6.36046677063471;-4.03124522185965]
Robot Arm 2 end: [-6.29614105939195;-3.78339589783067;0.0924843988745165]
Robot Arm 1 togoal: 7.37600124984328

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.881689e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.124561e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [-28.8287229563169;5.08943619933993;0.123869121537417]
Robot Arm 1 end: [4.81375961737275;-0.466409189184473;1.17966138957795]
Robot Arm 1 togoal: 11.6858599456268
Mobile Platform 2 Pose: [24.8614082184559;-6.63708013481725;-4.09533600103619]
Robot Arm 2 end: [-6.59004092436592;-4.43502986586406;0.248023940655528]
Robot Arm 1 togoal: 6.64960010368957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.078871e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.061101e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [-28.7861675071844;5.07396986179275;0.192472127763784]
Robot Arm 1 end: [4.47850273965718;-0.234815611951708;0.39890221220887]
Robot Arm 1 togoal: 11.6832138580097
Mobile Platform 2 Pose: [24.9822884594783;-6.7493465418666;-4.1150438751623]
Robot Arm 2 end: [-6.40230002328005;-4.61280913337479;0.556765166286482]
Robot Arm 1 togoal: 6.54942893468487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.001895e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.083493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [-28.7933892702924;5.09679570926879;0.204328029313146]
Robot Arm 1 end: [5.22641230240665;0.444021397993496;0.076364079055708]
Robot Arm 1 togoal: 10.7791854744551
Mobile Platform 2 Pose: [24.9122551551121;-6.73925166161034;-4.15209699948534]
Robot Arm 2 end: [-6.20061931646777;-4.39376115225119;0.983768635185544]
Robot Arm 1 togoal: 6.79366065797712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.048293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.472732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [-28.9203271468373;5.17516512986764;0.158015690332506]
Robot Arm 1 end: [5.05426662065591;-0.5056098984078;0.412963603654305]
Robot Arm 1 togoal: 11.6642986352677
Mobile Platform 2 Pose: [24.8287781085843;-6.66019355463493;-4.16631960891189]
Robot Arm 2 end: [-6.61041085572209;-4.04302790499105;1.40951087610813]
Robot Arm 1 togoal: 6.8547254454477

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.669765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.492323e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [-28.9628654231366;5.18233407273493;0.149350869325636]
Robot Arm 1 end: [4.6528636767616;-0.113912385216157;0.26752914133433]
Robot Arm 1 togoal: 11.5088783027995
Mobile Platform 2 Pose: [24.8060244223593;-6.64893567231943;-4.18816681383822]
Robot Arm 2 end: [-6.83128160023312;-3.92246383286632;1.3170543900772]
Robot Arm 1 togoal: 6.85702675885444

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.161191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.863371e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [-29.203748379381;5.58406405457709;0.221615756198956]
Robot Arm 1 end: [5.82735175926424;0.481440167941325;1.25197108449494]
Robot Arm 1 togoal: 10.3964585881335
Mobile Platform 2 Pose: [24.6254106036693;-6.53634843272816;-4.22449036819974]
Robot Arm 2 end: [-6.82355684527872;-3.85582058649462;1.19879753783263]
Robot Arm 1 togoal: 6.92420004095388

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.613505e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.603230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [-29.1256792473307;5.5400680754345;0.254695678315205]
Robot Arm 1 end: [5.98101317101976;0.834857433113742;0.599080923909836]
Robot Arm 1 togoal: 10.0499765266256
Mobile Platform 2 Pose: [24.210450726277;-6.28561637516304;-4.27729874985943]
Robot Arm 2 end: [-6.70431981709087;-3.74225060247038;0.637387440773102]
Robot Arm 1 togoal: 7.12753158850424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.547971e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.306060e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [-29.586823502963;5.80102492537105;0.259560204246369]
Robot Arm 1 end: [6.49545117409631;0.202761680809944;0.247216313696359]
Robot Arm 1 togoal: 10.4828558148838
Mobile Platform 2 Pose: [24.0545929751728;-6.19747395811767;-4.2641183981134]
Robot Arm 2 end: [-6.42607125796235;-3.6734544530672;0.458513658199688]
Robot Arm 1 togoal: 7.34350205536094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.615861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.792181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [-29.9208596610955;6.02193065677175;0.27965006828521]
Robot Arm 1 end: [6.54507159982156;-0.0891230273048726;1.17251775263516]
Robot Arm 1 togoal: 10.6699753320793
Mobile Platform 2 Pose: [24.0115170250214;-6.17002909243873;-4.17828097188517]
Robot Arm 2 end: [-5.95151511940004;-3.7908225363535;-0.0103868454681255]
Robot Arm 1 togoal: 7.56896692250501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024748e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.366691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [-29.9658938831301;6.04997465095736;0.277117842038733]
Robot Arm 1 end: [6.50221577107568;0.137124267988721;1.3588002531058]
Robot Arm 1 togoal: 10.4659983267293
Mobile Platform 2 Pose: [24.1185627050973;-6.20842277102926;-4.11892179912284]
Robot Arm 2 end: [-6.30353354547519;-4.41200412789986;-0.0283454391754689]
Robot Arm 1 togoal: 6.87677492767716

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.585608e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.245933e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [-30.8077546506064;6.57190019504589;0.279367963627566]
Robot Arm 1 end: [7.04860457981031;-0.0682475827839325;1.28623488761822]
Robot Arm 1 togoal: 10.4945442479673
Mobile Platform 2 Pose: [24.2009268668896;-6.2752688553205;-4.12098457461723]
Robot Arm 2 end: [-6.57532496020323;-4.71527822637369;0.284556632168287]
Robot Arm 1 togoal: 6.41759110217393

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.946404e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.748071e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [-30.9468639094941;6.69703239809706;0.300210999939859]
Robot Arm 1 end: [6.63523158580736;0.552824746007316;1.39962292621034]
Robot Arm 1 togoal: 10.0292332286592
Mobile Platform 2 Pose: [23.9782555329585;-6.20263053916903;-4.07623246728882]
Robot Arm 2 end: [-5.91585780621079;-4.70730529703271;0.86360386957171]
Robot Arm 1 togoal: 6.71751474506825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.659895e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.040946e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [-30.8553676287632;6.63883362866938;0.301809750182886]
Robot Arm 1 end: [6.2438347707619;0.775444599448976;1.05718480234073]
Robot Arm 1 togoal: 9.97077349581025
Mobile Platform 2 Pose: [23.9213369734268;-6.20140333057092;-4.03290704332479]
Robot Arm 2 end: [-5.52812558662058;-4.00619240143263;0.849353661908524]
Robot Arm 1 togoal: 7.50829535255195

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.995792e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.218599e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [-30.8007849948687;6.60666112917793;0.299273675643172]
Robot Arm 1 end: [6.33221061816568;0.783167329418865;0.813153440292215]
Robot Arm 1 togoal: 9.94503545388745
Mobile Platform 2 Pose: [23.4211428050099;-5.93214431331244;-4.1003035169523]
Robot Arm 2 end: [-5.58910982959731;-4.30070834012636;1.03184815832125]
Robot Arm 1 togoal: 7.22337504521798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.127386e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.826761e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [-30.7573109534;6.57849744105687;0.295984743254654]
Robot Arm 1 end: [6.5290526429681;0.810705434559757;0.971220887561893]
Robot Arm 1 togoal: 9.83833660931509
Mobile Platform 2 Pose: [23.3955951805219;-5.9162984687614;-4.10809823688569]
Robot Arm 2 end: [-5.57668279205301;-4.2584040424737;0.976437466214842]
Robot Arm 1 togoal: 7.26830156328182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.845542e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005571e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [-30.898455390641;6.65941802742044;0.306066592038134]
Robot Arm 1 end: [6.83613201081389;0.746218556490782;1.09229087565903]
Robot Arm 1 togoal: 9.78909205036531
Mobile Platform 2 Pose: [23.3508068435064;-5.93450691994803;-4.16015973230996]
Robot Arm 2 end: [-5.40679935444021;-4.42422479366314;0.490655367938305]
Robot Arm 1 togoal: 7.29714816779748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.974310e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.696232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [-31.2171048831739;6.84784604329551;0.311285049562183]
Robot Arm 1 end: [7.21643218919091;0.564499026982862;1.1600906970254]
Robot Arm 1 togoal: 9.84414465020468
Mobile Platform 2 Pose: [22.7758910880673;-5.68050953607429;-4.32322651160875]
Robot Arm 2 end: [-5.7784415812933;-4.53888454681503;0.0106954865601772]
Robot Arm 1 togoal: 7.06585856063407

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.772419e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.312403e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [-31.6819504135079;7.11843204394132;0.301020135189154]
Robot Arm 1 end: [7.67424760450029;-0.143685899762698;0.757146594872633]
Robot Arm 1 togoal: 10.4348914638698
Mobile Platform 2 Pose: [22.8595926315768;-5.74352045148878;-4.32511029858836]
Robot Arm 2 end: [-5.62016651962923;-4.54256636664774;-0.0495571488730807]
Robot Arm 1 togoal: 7.17169247361441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.602028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.049267e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [-32.1725089258419;7.41632557810125;0.26921446567015]
Robot Arm 1 end: [7.09653289439681;-0.327927216144912;0.6964219844913]
Robot Arm 1 togoal: 10.7599317246424
Mobile Platform 2 Pose: [22.8092195018188;-5.70159203948059;-4.33549360755993]
Robot Arm 2 end: [-5.68408654777843;-4.4544544279658;-0.0792498830657696]
Robot Arm 1 togoal: 7.20700938717491

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.182306e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.011494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [-32.2341411814365;7.42121638361817;0.248249669104061]
Robot Arm 1 end: [6.80826657048755;-0.179446482481869;0.553701309281454]
Robot Arm 1 togoal: 10.7118607035307
Mobile Platform 2 Pose: [22.4751744295958;-5.45181835873315;-4.34612913343851]
Robot Arm 2 end: [-5.53338492390458;-4.36378817427797;-0.0778180299861459]
Robot Arm 1 togoal: 7.3670722186918

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.686791e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.363259e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.556461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.259141566892;-10.0035524791063;1.47381471675359]
Robot Arm 1 togoal: 28.4706367203596
Mobile Platform 2 Pose: [-2.35428427305807;9.99836513540346;8.17369167832346]
Robot Arm 2 end: [10.4652565059268;8.15449630195842;1.33404529895789]
Robot Arm 1 togoal: 27.3577669390512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785398163816203;-10.0000000004024;-9.99999999998701]
Robot Arm 1 end: [-10.4383911944831;-9.89000075687976;1.3825988437274]
Robot Arm 1 togoal: 28.5194516007489
Mobile Platform 2 Pose: [-2.2638961533991;9.93602312817799;8.17275211815219]
Robot Arm 2 end: [10.4621568328558;7.90426106254084;0.100445217586139]
Robot Arm 1 togoal: 27.2264651091147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.448303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785398163816203;-10.0000000004024;-9.99999999998701]
Robot Arm 1 end: [-10.4804384713904;-9.87299582417385;1.36188030313921]
Robot Arm 1 togoal: 28.5378633062457
Mobile Platform 2 Pose: [-2.2638961533991;9.93602312817799;8.17275211815219]
Robot Arm 2 end: [10.4145310903417;7.7767752621961;0.162554446198769]
Robot Arm 1 togoal: 27.103730235593

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.495790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.78539816395399;-10.0000000004389;-9.99999999997532]
Robot Arm 1 end: [-10.5056015528964;-9.79946133434031;1.31710470095775]
Robot Arm 1 togoal: 28.5050861684218
Mobile Platform 2 Pose: [-2.34043849765419;9.95990433975244;8.15520946640024]
Robot Arm 2 end: [9.88422778452699;7.6841098908171;1.19169638605984]
Robot Arm 1 togoal: 26.6123786627127

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.166092e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.78539816395399;-10.0000000004389;-9.99999999997532]
Robot Arm 1 end: [-10.4258244910853;-9.70689428583554;1.30974479878516]
Robot Arm 1 togoal: 28.383456753781
Mobile Platform 2 Pose: [-2.34043849765392;9.95990433975222;8.15520946640016]
Robot Arm 2 end: [9.84062774037014;7.51753423229858;0.373179704190489]
Robot Arm 1 togoal: 26.492112149528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.520472e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.78539816395399;-10.0000000004389;-9.99999999997532]
Robot Arm 1 end: [-10.2905006741745;-9.81246969333648;1.41705190597371]
Robot Arm 1 togoal: 28.3592873352682
Mobile Platform 2 Pose: [-2.34043849692528;9.95990433963309;8.15520946613353]
Robot Arm 2 end: [9.66070267725926;7.6123205969281;0.384114698161165]
Robot Arm 1 togoal: 26.4202493319443

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.242793e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.78539816345844;-10.0000000000103;-9.99999999998014]
Robot Arm 1 end: [-10.2757265233968;-9.93179103344226;1.46568108308829]
Robot Arm 1 togoal: 28.4321022795529
Mobile Platform 2 Pose: [-2.34390433804104;9.95428704187292;8.15488503476732]
Robot Arm 2 end: [9.47124779053042;7.96468719752009;0.847512858999761]
Robot Arm 1 togoal: 26.5011898139323

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.647088e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.78539816345844;-10.0000000000103;-9.99999999998014]
Robot Arm 1 end: [-10.1370202981001;-9.97274462713127;1.50568202840378]
Robot Arm 1 togoal: 28.362128782356
Mobile Platform 2 Pose: [-2.34390433804104;9.95428704187292;8.15488503476732]
Robot Arm 2 end: [9.48772303863281;8.03617946994697;0.979325482544857]
Robot Arm 1 togoal: 26.558775016658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.903027e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.78539816345844;-10.0000000000103;-9.99999999998014]
Robot Arm 1 end: [-10.0551559489515;-10.0012133422537;1.51916954627355]
Robot Arm 1 togoal: 28.3241560977494
Mobile Platform 2 Pose: [-2.34390433973129;9.95428704314446;8.15488503466404]
Robot Arm 2 end: [9.38336751697981;8.08827346659133;0.933821267548875]
Robot Arm 1 togoal: 26.518773579567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785398161916148;-9.99999999854914;-10.0000000001798]
Robot Arm 1 end: [-10.157785318919;-10.0360315649013;1.50027934383029]
Robot Arm 1 togoal: 28.4214584280775
Mobile Platform 2 Pose: [-2.18454843103471;9.82730067630362;8.16379600035973]
Robot Arm 2 end: [9.6550463826762;8.2158723520262;1.42514752966207]
Robot Arm 1 togoal: 26.7982843022932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.245128e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785398161916148;-9.99999999854914;-10.0000000001798]
Robot Arm 1 end: [-10.1903456805628;-10.1074783980491;1.47938599692534]
Robot Arm 1 togoal: 28.4949553071045
Mobile Platform 2 Pose: [-2.18454842811828;9.82730067464765;8.16379599955045]
Robot Arm 2 end: [9.573679164922;8.44910153321156;1.32470260006011]
Robot Arm 1 togoal: 26.8986392983599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785398161916148;-9.99999999854914;-10.0000000001798]
Robot Arm 1 end: [-10.2626524831131;-10.0816341680801;1.43795456508952]
Robot Arm 1 togoal: 28.528126700789
Mobile Platform 2 Pose: [-1.92473831320876;9.69347493731711;8.08683145399407]
Robot Arm 2 end: [9.8769035395387;8.07854530802794;0.0693791434506079]
Robot Arm 1 togoal: 26.9078482581888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.728934e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.785398161715191;-9.99999999838838;-10.0000000001196]
Robot Arm 1 end: [-10.1679996219456;-10.1397941808266;1.47939727687927]
Robot Arm 1 togoal: 28.5019516732351
Mobile Platform 2 Pose: [-1.92473831248677;9.69347493722079;8.08683145421555]
Robot Arm 2 end: [9.74104092086935;7.99115823218417;0.104091412835714]
Robot Arm 1 togoal: 26.7469269472085

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.292141e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785398161715191;-9.99999999838838;-10.0000000001196]
Robot Arm 1 end: [-10.2360643236917;-10.1388486707872;1.4382584363819]
Robot Arm 1 togoal: 28.5495774270619
Mobile Platform 2 Pose: [-1.92473831248678;9.6934749372208;8.08683145421556]
Robot Arm 2 end: [9.84050169726911;7.89205980291569;0.223365645955158]
Robot Arm 1 togoal: 26.7479936912518

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.829664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785398161715191;-9.99999999838838;-10.0000000001196]
Robot Arm 1 end: [-10.1333954544023;-10.1731008002333;1.47940059519288]
Robot Arm 1 togoal: 28.5010410148507
Mobile Platform 2 Pose: [-1.92473831248678;9.6934749372208;8.08683145421556]
Robot Arm 2 end: [9.76196962401141;7.80614858422286;0.293233226415187]
Robot Arm 1 togoal: 26.6289650958142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.745707e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785398161889473;-9.9999999985278;-10.0000000000673]
Robot Arm 1 end: [-10.1701151050552;-10.2289706063629;1.43425528500164]
Robot Arm 1 togoal: 28.5666294789892
Mobile Platform 2 Pose: [-1.92473831239848;9.69347493715016;8.08683145430682]
Robot Arm 2 end: [9.71685311232727;7.73965385911846;0.360597849795746]
Robot Arm 1 togoal: 26.5479970778329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.612863e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785398161889473;-9.9999999985278;-10.0000000000673]
Robot Arm 1 end: [-9.91744542544512;-10.1437430246614;1.42892544693122]
Robot Arm 1 togoal: 28.3281395972715
Mobile Platform 2 Pose: [-1.92473831239848;9.69347493715016;8.08683145430682]
Robot Arm 2 end: [9.65846314503872;8.08424422062703;-0.150218771486764]
Robot Arm 1 togoal: 26.7635579555166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.839903e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785398161889473;-9.9999999985278;-10.0000000000673]
Robot Arm 1 end: [-9.99184030273135;-10.3197356701252;1.38707374109319]
Robot Arm 1 togoal: 28.5058462887264
Mobile Platform 2 Pose: [-1.9234918439502;9.69315028733968;8.07937263404903]
Robot Arm 2 end: [9.98436113036831;7.47292707824444;0.453878761214924]
Robot Arm 1 togoal: 26.567209454681

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.986875e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785398162385532;-9.99999999880098;-9.99999999950601]
Robot Arm 1 end: [-9.92622289753719;-10.2849502575927;1.43612004598652]
Robot Arm 1 togoal: 28.4348513363784
Mobile Platform 2 Pose: [-1.92620119683934;9.69729692401176;8.0777259221082]
Robot Arm 2 end: [10.2226655321816;7.47106751896356;0.62799616955263]
Robot Arm 1 togoal: 26.7393316543227

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.068423e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785398162417309;-9.99999999890299;-9.99999999922901]
Robot Arm 1 end: [-9.78282123049062;-10.1460633924089;1.44243374216251]
Robot Arm 1 togoal: 28.2352626817138
Mobile Platform 2 Pose: [-1.96829422142522;9.72099915226274;8.07034903248235]
Robot Arm 2 end: [10.2345565976337;7.54005438242908;1.00470288036955]
Robot Arm 1 togoal: 26.7835276085348

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.596025e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.785398161825153;-9.99999999820492;-9.99999999916348]
Robot Arm 1 end: [-9.79872939659852;-10.0368175146117;1.46464313054804]
Robot Arm 1 togoal: 28.1685448316082
Mobile Platform 2 Pose: [-1.9655557864527;9.71846040167623;8.07083992652653]
Robot Arm 2 end: [10.0341883757279;7.59213359558297;1.23267096503901]
Robot Arm 1 togoal: 26.6633644154557

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.663891e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.78539816118284;-9.99999999801104;-9.99999999983846]
Robot Arm 1 end: [-9.70215582755861;-10.1559560851588;1.41418834489705]
Robot Arm 1 togoal: 28.1859702456722
Mobile Platform 2 Pose: [-1.96555578619096;9.7184604013503;8.07083992660505]
Robot Arm 2 end: [10.0717362572192;7.74848378554629;1.34356229899906]
Robot Arm 1 togoal: 26.7939313502685

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.624581e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.78539816248806;-9.99999999884893;-9.99999999965796]
Robot Arm 1 end: [-9.77410354504952;-10.0204179696807;1.45792465201584]
Robot Arm 1 togoal: 28.1395857323909
Mobile Platform 2 Pose: [-1.96555578619096;9.7184604013503;8.07083992660505]
Robot Arm 2 end: [10.0659077267014;7.83931958848693;1.39932850173981]
Robot Arm 1 togoal: 26.8495210428446

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.179068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785398163766391;-10.0000000000101;-9.99999999958939]
Robot Arm 1 end: [-9.77908065851501;-10.1252385428276;1.47461848211119]
Robot Arm 1 togoal: 28.2177144600153
Mobile Platform 2 Pose: [-1.9655557861809;9.71846040134828;8.07083992660807]
Robot Arm 2 end: [10.0159624715606;8.006857182344;1.45693368730936]
Robot Arm 1 togoal: 26.9237769219082

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.332896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785398166961358;-10.0000000023566;-9.99999999974687]
Robot Arm 1 end: [-9.91017275585103;-10.0098207413922;1.50422586052618]
Robot Arm 1 togoal: 28.2277910337
Mobile Platform 2 Pose: [-1.96555578615655;9.71846040032916;8.07083992668838]
Robot Arm 2 end: [10.2059990200717;8.11510970357247;1.37127869877248]
Robot Arm 1 togoal: 27.1378335988643

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.827731e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.78539816842632;-10.000000003802;-9.99999999983791]
Robot Arm 1 end: [-9.75924127751911;-10.0322103617448;1.47814438220962]
Robot Arm 1 togoal: 28.1374999679145
Mobile Platform 2 Pose: [-1.96555578615655;9.71846040032916;8.07083992668838]
Robot Arm 2 end: [10.3429420384046;8.2618379256242;1.22921045764463]
Robot Arm 1 togoal: 27.3388945704957

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.153515e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785398169199109;-10.0000000044979;-9.99999999938614]
Robot Arm 1 end: [-9.71332204026971;-10.2188281831471;1.43073968148811]
Robot Arm 1 togoal: 28.2387008534051
Mobile Platform 2 Pose: [-1.89941872168034;9.68072955732949;8.07510078741385]
Robot Arm 2 end: [10.4231112177568;8.47887291221199;0.760474051574111]
Robot Arm 1 togoal: 27.5526879929486

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.555695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785398167564163;-10.0000000031899;-10.0000000004243]
Robot Arm 1 end: [-9.72625649456327;-10.0630108972018;1.46618544887568]
Robot Arm 1 togoal: 28.1363218311412
Mobile Platform 2 Pose: [-1.85497182597895;9.65672859073147;8.07832674799076]
Robot Arm 2 end: [10.3525331320157;8.35229363635186;0.372538025037129]
Robot Arm 1 togoal: 27.4290220662825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.830141e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785398168242581;-10.0000000035427;-10.0000000008066]
Robot Arm 1 end: [-9.71332203820518;-10.2188281880542;1.43073968047976]
Robot Arm 1 togoal: 28.2387008554806
Mobile Platform 2 Pose: [-1.8313946444079;9.63918734084397;8.07474494791908]
Robot Arm 2 end: [9.96228419463745;8.68093559479006;1.02189160120672]
Robot Arm 1 togoal: 27.344455638354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.710962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785398169805251;-10.0000000047928;-10.0000000012521]
Robot Arm 1 end: [-9.93137555029059;-10.1568291790665;1.47790522530154]
Robot Arm 1 togoal: 28.3471224472678
Mobile Platform 2 Pose: [-1.8175490018803;9.63018801726223;8.07121450342521]
Robot Arm 2 end: [9.61509006138506;8.55185177185645;1.30398138515486]
Robot Arm 1 togoal: 26.9994455380174

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.788553e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785398169451333;-10.0000000048751;-10.0000000015856]
Robot Arm 1 end: [-9.94753997523292;-10.3175654417276;1.40997355126137]
Robot Arm 1 togoal: 28.4731477609118
Mobile Platform 2 Pose: [-1.81203712030585;9.62732919958546;8.05818458127969]
Robot Arm 2 end: [9.1442262401137;8.45357678318783;1.22468140765455]
Robot Arm 1 togoal: 26.5917975912467

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.139484e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785398169451333;-10.0000000048751;-10.0000000015856]
Robot Arm 1 end: [-9.77402944669557;-10.4119874529187;1.35455826940337]
Robot Arm 1 togoal: 28.419872891809
Mobile Platform 2 Pose: [-1.81203711883123;9.62732919924801;8.05818458166601]
Robot Arm 2 end: [9.20905548348835;8.44679404889394;1.26056929610373]
Robot Arm 1 togoal: 26.6334347653767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.415439e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785398169451333;-10.0000000048751;-10.0000000015856]
Robot Arm 1 end: [-9.85924466628165;-10.575903971169;1.22581454958749]
Robot Arm 1 togoal: 28.5979817014443
Mobile Platform 2 Pose: [-1.81203711883123;9.62732919924801;8.05818458166601]
Robot Arm 2 end: [9.15701430067214;8.23502451637043;1.28272377700558]
Robot Arm 1 togoal: 26.4492739330824

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.622347e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.052557e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785398169873206;-10.0000000050173;-10.0000000013954]
Robot Arm 1 end: [-9.97721142126612;-10.6967486127921;1.04681662021146]
Robot Arm 1 togoal: 28.7692410255944
Mobile Platform 2 Pose: [-1.81203711883122;9.62732919924801;8.05818458166601]
Robot Arm 2 end: [9.20138139163583;7.98336023095295;1.28438483472492]
Robot Arm 1 togoal: 26.3087491013867

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.116590e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.245317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785398169796008;-10.0000000050019;-10.0000000014186]
Robot Arm 1 end: [-10.123983548346;-10.7374697425168;0.837775330762687]
Robot Arm 1 togoal: 28.9047428828653
Mobile Platform 2 Pose: [-1.81203711860184;9.62732920177742;8.05818457859419]
Robot Arm 2 end: [9.19795138983388;7.88108768724206;1.2865070526075]
Robot Arm 1 togoal: 26.2364178459568

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.011729e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006502e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.785398170165017;-10.0000000053224;-10.0000000015546]
Robot Arm 1 end: [-10.2700685075303;-10.6761719230283;0.620081831661879]
Robot Arm 1 togoal: 28.9688007386678
Mobile Platform 2 Pose: [-1.81203710608941;9.62732919235649;8.05818457420404]
Robot Arm 2 end: [9.212757921963;7.82478748035262;1.29214217665093]
Robot Arm 1 togoal: 26.2088819218027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.974425e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.769267e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785398168859478;-10.0000000041279;-10.0000000009459]
Robot Arm 1 end: [-10.3393432278032;-10.5421264465035;0.417072171118482]
Robot Arm 1 togoal: 28.9289906241801
Mobile Platform 2 Pose: [-1.81203711553494;9.62732919661135;8.05818457512707]
Robot Arm 2 end: [9.11446007762377;7.80322678772281;1.20386842055038]
Robot Arm 1 togoal: 26.1231322883158

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.119084e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.546052e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785398168859478;-10.0000000041279;-10.0000000009459]
Robot Arm 1 end: [-10.3147781850072;-10.3885068926345;0.264177343428254]
Robot Arm 1 togoal: 28.8090442258524
Mobile Platform 2 Pose: [-1.81203711408138;9.62732919416108;8.05818457521721]
Robot Arm 2 end: [8.9765692199527;7.98328307043035;1.09060607645849]
Robot Arm 1 togoal: 26.1475407692636

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.096111e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.577820e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785398168859478;-10.0000000041279;-10.0000000009459]
Robot Arm 1 end: [-10.3757680609556;-10.185764903737;0.191901888752494]
Robot Arm 1 togoal: 28.7124281559767
Mobile Platform 2 Pose: [-1.81203711408138;9.62732919416108;8.05818457521721]
Robot Arm 2 end: [8.94090549143886;7.79100109570185;0.954323524867444]
Robot Arm 1 togoal: 25.992282374781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.970015e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.225775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785398168859478;-10.0000000041279;-10.0000000009459]
Robot Arm 1 end: [-10.4005502896329;-10.0902505596134;0.155909832080931]
Robot Arm 1 togoal: 28.6646836862791
Mobile Platform 2 Pose: [-1.14361643621512;9.16000514160691;8.00868817559232]
Robot Arm 2 end: [8.67300697175924;8.06353919208072;1.23254626177731]
Robot Arm 1 togoal: 25.9818367916955

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.008625e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.198465e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.683812334839563;-9.91873133691192;-10.0003082709262]
Robot Arm 1 end: [-9.69686616317287;-9.46339146501462;1.0774002719546]
Robot Arm 1 togoal: 27.6945287861893
Mobile Platform 2 Pose: [-1.14361643621512;9.16000514160691;8.00868817559232]
Robot Arm 2 end: [8.50678613297537;7.91555293324754;1.08793933952635]
Robot Arm 1 togoal: 25.7615160930279

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.275732e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.829655e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.719173993027297;-9.93907908225471;-9.99844921685379]
Robot Arm 1 end: [-9.42353610791757;-9.42950546895729;0.911709676295514]
Robot Arm 1 togoal: 27.4800049563747
Mobile Platform 2 Pose: [-1.14361643621499;9.16000514160681;8.00868817559229]
Robot Arm 2 end: [8.42287108606967;7.75716157440196;0.89102664879098]
Robot Arm 1 togoal: 25.5952306623049

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.525511e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.839103e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.719173992697368;-9.93907908218324;-9.99844921695277]
Robot Arm 1 end: [-9.42459710616401;-9.40798256683483;0.945537457533935]
Robot Arm 1 togoal: 27.4648123457842
Mobile Platform 2 Pose: [-1.14361643621499;9.16000514160681;8.00868817559229]
Robot Arm 2 end: [9.02041071545495;7.18925769180342;0.676174899235171]
Robot Arm 1 togoal: 25.6507374792753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.030017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.319132e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.719173992697368;-9.93907908218324;-9.99844921695277]
Robot Arm 1 end: [-10.4507286203069;-10.3229750682151;1.06400207728214]
Robot Arm 1 togoal: 28.8351255212475
Mobile Platform 2 Pose: [-1.14361643621499;9.16000514160681;8.00868817559229]
Robot Arm 2 end: [8.90560464226569;7.26287182027278;0.751120836305502]
Robot Arm 1 togoal: 25.6129136237859

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.150490e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.696767e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.719173992697368;-9.93907908218324;-9.99844921695277]
Robot Arm 1 end: [-10.3451311940256;-10.2320123790438;1.19206005811621]
Robot Arm 1 togoal: 28.694370348437
Mobile Platform 2 Pose: [-1.14361643714661;9.16000514189849;8.00868817544736]
Robot Arm 2 end: [8.75215430636294;7.33749815013769;0.662343702625092]
Robot Arm 1 togoal: 25.5532664031958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.907094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.400582e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.719173992697368;-9.93907908218324;-9.99844921695277]
Robot Arm 1 end: [-10.1805065035046;-10.0479286787336;1.29638737758313]
Robot Arm 1 togoal: 28.4468405605215
Mobile Platform 2 Pose: [-0.14874991935801;8.54088862285036;7.71022822011077]
Robot Arm 2 end: [8.75030113384272;8.14637231819815;-0.0199896259558817]
Robot Arm 1 togoal: 26.1388459949508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.384665e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.980922e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.719173992644576;-9.93907908206618;-9.99844921706641]
Robot Arm 1 end: [-9.94374767973005;-9.95184008179768;1.29913900110755]
Robot Arm 1 togoal: 28.2113136253028
Mobile Platform 2 Pose: [-0.233094601562971;8.59149474207396;7.71171076131967]
Robot Arm 2 end: [8.70178024768312;7.84308615169296;-0.125382163730413]
Robot Arm 1 togoal: 25.9006347813797

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.752937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.086820e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.519917338713482;-9.80699821033425;-9.95131502132747]
Robot Arm 1 end: [-9.95710109575088;-9.46994553251701;1.13061513062963]
Robot Arm 1 togoal: 27.8839929624764
Mobile Platform 2 Pose: [-0.233094601562971;8.59149474207396;7.71171076131967]
Robot Arm 2 end: [8.95897436441349;7.76932447120607;1.2934405873755]
Robot Arm 1 togoal: 25.9854456263456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.745498e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.425978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.519917338928557;-9.80699821017977;-9.95131502072177]
Robot Arm 1 end: [-9.92854202604177;-9.52580658105433;1.22434493748979]
Robot Arm 1 togoal: 27.9014679569117
Mobile Platform 2 Pose: [-0.23309460181239;8.59149474213411;7.71171076123602]
Robot Arm 2 end: [9.05600988388875;7.64447867337222;1.20937774951856]
Robot Arm 1 togoal: 25.9722207132341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.668397e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.51991734064556;-9.80699821181362;-9.95131501998464]
Robot Arm 1 end: [-10.0219454469488;-9.61321759528566;1.25768137489623]
Robot Arm 1 togoal: 28.0290196157294
Mobile Platform 2 Pose: [-0.425569149661948;8.67795682224265;7.66529879334894]
Robot Arm 2 end: [8.66999513036336;7.91417675315516;-0.138115177758202]
Robot Arm 1 togoal: 25.9275743193254

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.599545e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.130009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.224207957348986;-9.62646231521555;-9.95527724089417]
Robot Arm 1 end: [-8.95040613879796;-10.0509964240273;0.904167467034089]
Robot Arm 1 togoal: 27.596029294003
Mobile Platform 2 Pose: [-0.262042863246761;8.57641552354475;7.6747537785816]
Robot Arm 2 end: [8.78906473828857;7.73893078376192;-0.0436155948896191]
Robot Arm 1 togoal: 25.8871906751286

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.485708e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.126487307120284;-9.53556116012051;-9.94330367891967]
Robot Arm 1 end: [-10.2088574407632;-9.89015328441946;0.975032374771571]
Robot Arm 1 togoal: 28.3604336600587
Mobile Platform 2 Pose: [-0.201836802771684;8.52603634884507;7.64630830600355]
Robot Arm 2 end: [8.18349007228086;7.83568647439788;0.0299051989718546]
Robot Arm 1 togoal: 25.5141997116784

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.339202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.090503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.00467653350967201;-9.45597818184871;-9.94240689741357]
Robot Arm 1 end: [-9.96486787640463;-9.27127077076251;0.587498856614509]
Robot Arm 1 togoal: 27.7641720717013
Mobile Platform 2 Pose: [-0.0960934333139675;8.46674133251387;7.63145868556508]
Robot Arm 2 end: [8.87064926512859;8.35969403686877;0.549430415158852]
Robot Arm 1 togoal: 26.3462277427552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.906763e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-0.00839723208033194;-9.44892420123344;-9.94128467589953]
Robot Arm 1 end: [-9.56234059967003;-9.26760406576657;0.304671560089623]
Robot Arm 1 togoal: 27.4846355450891
Mobile Platform 2 Pose: [-0.432919505444927;8.65108828231155;7.60480523235564]
Robot Arm 2 end: [8.65401486106334;7.68125081696638;0.702438664701522]
Robot Arm 1 togoal: 25.7151500782883

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.967643e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.136707e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-0.00839723236475578;-9.44892420110145;-9.94128467600552]
Robot Arm 1 end: [-9.79570788860602;-9.30021986783255;0.435293039245674]
Robot Arm 1 togoal: 27.668525391902
Mobile Platform 2 Pose: [-0.421003098717843;8.64321932303652;7.59245542087952]
Robot Arm 2 end: [8.74506743135555;7.4896561082123;0.635622402596037]
Robot Arm 1 togoal: 25.6524758508722

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.769829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.255426e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-0.00839723265213862;-9.44892420087502;-9.9412846759363]
Robot Arm 1 end: [-9.97690579570985;-9.40708053027442;0.616255159488878]
Robot Arm 1 togoal: 27.8662897262204
Mobile Platform 2 Pose: [-0.421003098325539;8.64321932260217;7.5924554206235]
Robot Arm 2 end: [8.70604249968446;7.44610095677762;0.567705516215962]
Robot Arm 1 togoal: 25.596703595715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.872556e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.610732e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-0.00839723241202031;-9.4489242012708;-9.94128467547249]
Robot Arm 1 end: [-9.81543310940292;-9.31592452033114;0.422606306824255]
Robot Arm 1 togoal: 27.6940931986147
Mobile Platform 2 Pose: [-0.421003098325539;8.64321932260217;7.5924554206235]
Robot Arm 2 end: [8.66242237517359;7.45210463020171;0.480858618663938]
Robot Arm 1 togoal: 25.5723260386312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.397963e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.356542e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-0.00839724482453017;-9.44892419553371;-9.94128467901673]
Robot Arm 1 end: [-9.7010351827653;-9.23337396184935;0.637781519753486]
Robot Arm 1 togoal: 27.5469333340891
Mobile Platform 2 Pose: [-0.421003098325539;8.64321932260217;7.5924554206235]
Robot Arm 2 end: [8.71260972424922;7.4891717337422;0.383221800790698]
Robot Arm 1 togoal: 25.6383975680032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.375017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.568509e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.200202389750292;-9.57836121062076;-9.89533734397766]
Robot Arm 1 end: [-9.67554459528946;-9.94895163630755;-0.154647636508217]
Robot Arm 1 togoal: 28.0694766875105
Mobile Platform 2 Pose: [-0.421003098325539;8.64321932260217;7.5924554206235]
Robot Arm 2 end: [8.75025843779512;7.58225225325705;0.299242592246127]
Robot Arm 1 togoal: 25.7332562444917

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.411534e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.233301e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.200202389750297;-9.57836121062076;-9.89533734397766]
Robot Arm 1 end: [-9.82298252774684;-9.93564306295732;-0.0977655084616435]
Robot Arm 1 togoal: 28.1602717032414
Mobile Platform 2 Pose: [-0.421003098325539;8.64321932260217;7.5924554206235]
Robot Arm 2 end: [8.67105607366479;7.6167280209605;0.246629459799997]
Robot Arm 1 togoal: 25.7017793419864

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.488400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007707e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.200202389657506;-9.5783612106338;-9.89533734385635]
Robot Arm 1 end: [-9.89795775488841;-9.90193002053042;-0.0562794928942503]
Robot Arm 1 togoal: 28.1869358383652
Mobile Platform 2 Pose: [-0.421003098325539;8.64321932260217;7.5924554206235]
Robot Arm 2 end: [8.6222091883407;7.75655785268187;0.237128078165118]
Robot Arm 1 togoal: 25.7629646768697

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.898289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.044858e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.113243117088044;-9.52574001114834;-9.88499495034019]
Robot Arm 1 end: [-9.69063117144517;-9.95182077670032;-0.0265787524773756]
Robot Arm 1 togoal: 28.0747414632901
Mobile Platform 2 Pose: [-0.421003098529431;8.64321932269688;7.59245542068467]
Robot Arm 2 end: [8.65594812440112;7.92396035345399;0.29905320182793]
Robot Arm 1 togoal: 25.8999210259433

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.843082e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-0.181384710225383;-9.29003774862439;-9.79660660089006]
Robot Arm 1 end: [-9.27968430476536;-9.73789051476986;-0.00996513226417922]
Robot Arm 1 togoal: 27.6339357149984
Mobile Platform 2 Pose: [-0.421003098431192;8.64321932261829;7.5924554206552]
Robot Arm 2 end: [8.71976602653712;8.04927873367289;0.394679766450211]
Robot Arm 1 togoal: 26.0283441686676

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.579171e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-0.181384709586528;-9.29003774902627;-9.79660660057803]
Robot Arm 1 end: [-9.3207054228662;-9.6313733739451;0.0189789516036384]
Robot Arm 1 togoal: 27.5850783180733
Mobile Platform 2 Pose: [-0.42100309791082;8.64321932221141;7.59245542055831]
Robot Arm 2 end: [8.74151052338976;8.0244312640381;0.281027765367642]
Robot Arm 1 togoal: 26.0319394251182

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.948300e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-0.289849640312224;-9.22159960737849;-9.78656682460073]
Robot Arm 1 end: [-9.92226126932476;-9.63564319392902;0.822014536677339]
Robot Arm 1 togoal: 27.9811286119853
Mobile Platform 2 Pose: [-0.421003097842816;8.64321932215701;7.59245542057872]
Robot Arm 2 end: [8.7310423396188;7.87977299248425;0.183866050703834]
Robot Arm 1 togoal: 25.9292529122943

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.530000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-0.561166617284153;-9.06699755021175;-9.76923774968663]
Robot Arm 1 end: [-9.07358052503931;-9.94929785960819;1.40471246976321]
Robot Arm 1 togoal: 27.6005340876987
Mobile Platform 2 Pose: [-0.42100309782898;8.64321932234134;7.59245542065399]
Robot Arm 2 end: [8.70274170190579;7.69837136117385;0.176083676219396]
Robot Arm 1 togoal: 25.784369217109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.920022e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-0.561166617284153;-9.06699755021175;-9.76923774968663]
Robot Arm 1 end: [-9.05173733053005;-9.73376964509875;1.4071391280346]
Robot Arm 1 togoal: 27.429971272597
Mobile Platform 2 Pose: [-0.421003096944855;8.64321932155668;7.59245542057734]
Robot Arm 2 end: [8.68540136084978;7.57369261711808;0.207615005430255]
Robot Arm 1 togoal: 25.6847012122189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.151007e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.527239e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-0.561166617284153;-9.06699755021175;-9.76923774968663]
Robot Arm 1 end: [-9.0758285490653;-9.54712020726038;1.35290002169891]
Robot Arm 1 togoal: 27.3130994365914
Mobile Platform 2 Pose: [-0.238402662196208;8.51478875401478;7.55828632898684]
Robot Arm 2 end: [8.50483025356294;7.75611069021813;0.625815376178522]
Robot Arm 1 togoal: 25.6614412118101

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.077391e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015712e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-0.561166617284153;-9.06699755021175;-9.76923774968663]
Robot Arm 1 end: [-9.15889621849899;-9.36230092622693;1.27726261709003]
Robot Arm 1 togoal: 27.2400699224655
Mobile Platform 2 Pose: [-0.187160063872207;8.4824998083107;7.56206358515257]
Robot Arm 2 end: [8.49207753317217;7.62964757904267;0.662414988467456]
Robot Arm 1 togoal: 25.5636181608966

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.310443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.079393e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-0.467107804806893;-9.12513435876735;-9.75256891537743]
Robot Arm 1 end: [-9.61955514495358;-9.18524838991414;0.31692537607752]
Robot Arm 1 togoal: 27.4672622510608
Mobile Platform 2 Pose: [0.258296616085444;8.12613446434457;7.51892389391929]
Robot Arm 2 end: [8.1862365854991;7.77240392842282;0.732628644418936]
Robot Arm 1 togoal: 25.4405006226895

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.703765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.387483e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-0.467107804806892;-9.12513435876735;-9.75256891537743]
Robot Arm 1 end: [-9.48339867286027;-9.1539036928982;0.248320731919528]
Robot Arm 1 togoal: 27.3513173913403
Mobile Platform 2 Pose: [0.258296615313249;8.12613446446934;7.51892389377454]
Robot Arm 2 end: [8.1924958323083;7.72601579601045;0.655321383342767]
Robot Arm 1 togoal: 25.4151124348547

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.394078e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.009400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-0.442000456530431;-9.14522023738852;-9.74503671089449]
Robot Arm 1 end: [-9.59656239645366;-9.24731565468665;1.02760796717919]
Robot Arm 1 togoal: 27.4723106666931
Mobile Platform 2 Pose: [0.258296613867037;8.12613446544253;7.51892389367994]
Robot Arm 2 end: [8.16122308949085;7.67360620882324;0.618912373813643]
Robot Arm 1 togoal: 25.3574474783273

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001775e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.971000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-0.380614851085935;-9.1881776991933;-9.73038472465618]
Robot Arm 1 end: [-9.5763849622262;-9.09233105461256;0.3078462925778]
Robot Arm 1 togoal: 27.3719508841813
Mobile Platform 2 Pose: [0.33634425472581;8.07418120223532;7.47873106042507]
Robot Arm 2 end: [8.24572470366663;7.21976687127812;0.0216409851761466]
Robot Arm 1 togoal: 25.1331438248081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.860076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-0.369920902539393;-9.19470928916569;-9.72940971712753]
Robot Arm 1 end: [-9.6363496825434;-9.11118351376849;0.389033722024368]
Robot Arm 1 togoal: 27.4245312048635
Mobile Platform 2 Pose: [0.328980279094166;8.0530170771952;7.43975542406859]
Robot Arm 2 end: [8.75825805584021;7.62478211595497;0.513442355854844]
Robot Arm 1 togoal: 25.7588889964349

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.017143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.167749e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-0.369920902539393;-9.19470928916569;-9.72940971712753]
Robot Arm 1 end: [-9.66090888310241;-9.27866138525114;0.21315138367381]
Robot Arm 1 togoal: 27.5668023339678
Mobile Platform 2 Pose: [0.328980279094166;8.0530170771952;7.43975542406859]
Robot Arm 2 end: [7.37469827212485;7.27344443449448;0.459686463973059]
Robot Arm 1 togoal: 24.5230179443865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000746e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.598918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-0.573532662980633;-9.06274435471777;-9.75127616606928]
Robot Arm 1 end: [-9.07825706022719;-9.59237302575317;-0.132812762439972]
Robot Arm 1 togoal: 27.3966434055265
Mobile Platform 2 Pose: [0.32898027908367;8.05301707728582;7.43975542396126]
Robot Arm 2 end: [7.42627318761405;7.17913533512297;0.371122650145592]
Robot Arm 1 togoal: 24.4973448763644

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.854231e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.789034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-0.573532662980633;-9.06274435471777;-9.75127616606928]
Robot Arm 1 end: [-9.09919224009834;-9.41338079305515;-0.0414647677511272]
Robot Arm 1 togoal: 27.2781926489065
Mobile Platform 2 Pose: [0.32898027908367;8.05301707728582;7.43975542396126]
Robot Arm 2 end: [7.41039097098326;7.29903357343065;0.296247760448446]
Robot Arm 1 togoal: 24.5739352645331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.595429e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.612407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-0.573532663466788;-9.06274435446019;-9.75127616605994]
Robot Arm 1 end: [-9.22882308475086;-9.28685771530218;0.102374774850919]
Robot Arm 1 togoal: 27.2716522287226
Mobile Platform 2 Pose: [0.328980279083674;8.05301707728582;7.43975542396126]
Robot Arm 2 end: [7.483386031834;7.17078197692392;0.228524155903193]
Robot Arm 1 togoal: 24.5392549733026

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990503e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.571131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-0.490182588558452;-9.11420449119222;-9.74003590036158]
Robot Arm 1 end: [-9.47441024033016;-9.35737578021389;0.131474043335124]
Robot Arm 1 togoal: 27.4934798395789
Mobile Platform 2 Pose: [0.342985324956812;8.05651185393957;7.4332424510486]
Robot Arm 2 end: [8.0029373508974;7.12445474458369;-0.0765480128529069]
Robot Arm 1 togoal: 24.8978887300309

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.945388e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.696181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-0.490182588687984;-9.11420449113999;-9.74003590044622]
Robot Arm 1 end: [-9.50093506637317;-9.47506768822104;0.105888929026443]
Robot Arm 1 togoal: 27.5965082803402
Mobile Platform 2 Pose: [0.342985325006558;8.05651185393781;7.43324245102549]
Robot Arm 2 end: [8.02261604649464;7.22301882769839;-0.0697024378181282]
Robot Arm 1 togoal: 24.9795396463646

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.003427e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.231444e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-0.652483658334279;-9.00606349700371;-9.75533266590366]
Robot Arm 1 end: [-9.20476648402341;-9.34989833212199;0.860528032682227]
Robot Arm 1 togoal: 27.2704573556979
Mobile Platform 2 Pose: [0.342985325006558;8.05651185393781;7.43324245102549]
Robot Arm 2 end: [7.93512544408645;7.3123130000249;-0.0726683381574031]
Robot Arm 1 togoal: 24.9784844391727

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.973947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.222253e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-0.637631777834159;-9.0154987362939;-9.75298144595958]
Robot Arm 1 end: [-9.44440705375839;-9.40814126120771;0.755609750109538]
Robot Arm 1 togoal: 27.4835720953785
Mobile Platform 2 Pose: [0.342985326082522;8.05651185321213;7.43324245121139]
Robot Arm 2 end: [8.00658855470289;7.33980512761166;-0.0681669526571571]
Robot Arm 1 togoal: 25.0485832901847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.567332e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.787577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-0.662616524512129;-9.00589129737769;-9.74218436879904]
Robot Arm 1 end: [-8.79574915300417;-9.33519893528196;0.091837861715912]
Robot Arm 1 togoal: 27.0031962646478
Mobile Platform 2 Pose: [0.342985325653369;8.05651185355545;7.43324245119954]
Robot Arm 2 end: [8.0012294600689;7.43813294895022;-0.0262744951832388]
Robot Arm 1 togoal: 25.1102927339121

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.407308e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.929285e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-1.3469708066587;-8.59343683680952;-9.73090270668789]
Robot Arm 1 end: [-8.76882820778345;-10.5028147862367;0.444769307812201]
Robot Arm 1 togoal: 27.8171277984414
Mobile Platform 2 Pose: [0.340415445372229;8.05873061673301;7.42888971194671]
Robot Arm 2 end: [8.25196942427864;7.62402535634506;0.103800758627344]
Robot Arm 1 togoal: 25.4115940333471

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002635e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.657218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-1.34697080668137;-8.59343683670999;-9.7309027065997]
Robot Arm 1 end: [-8.50064628652863;-10.425900224221;0.325860547191258]
Robot Arm 1 togoal: 27.5847724533073
Mobile Platform 2 Pose: [0.34041544539254;8.05873061671677;7.42888971194062]
Robot Arm 2 end: [8.20927923529623;7.63652435750531;0.153121436166069]
Robot Arm 1 togoal: 25.386924458218

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005443e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.771372e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-1.34697080673998;-8.59343683669827;-9.7309027066267]
Robot Arm 1 end: [-8.29320231421067;-10.2849062160766;0.257705280633551]
Robot Arm 1 togoal: 27.3443702551667
Mobile Platform 2 Pose: [0.34041544539254;8.05873061671677;7.42888971194062]
Robot Arm 2 end: [8.17924439322415;7.67593958100979;0.231445156057012]
Robot Arm 1 togoal: 25.3887124225632

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.313978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.715644e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-1.33765462835513;-8.58507304691518;-9.73017108449713]
Robot Arm 1 end: [-8.07603147033313;-9.41050978379225;0.398227725729159]
Robot Arm 1 togoal: 26.5475313609955
Mobile Platform 2 Pose: [0.34041544528396;8.05873061680363;7.42888971190804]
Robot Arm 2 end: [8.25132369343537;7.80343761575469;0.31372063562468]
Robot Arm 1 togoal: 25.5251000186928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.607130e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.615757e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-1.81083099624097;-8.28299524821653;-9.70650735441174]
Robot Arm 1 end: [-8.35174896797872;-10.0254246548131;1.02405446390421]
Robot Arm 1 togoal: 27.1670991763043
Mobile Platform 2 Pose: [0.294741828898251;8.0858004657158;7.42676718379552]
Robot Arm 2 end: [8.36049210012172;7.7540258597292;0.204667872674134]
Robot Arm 1 togoal: 25.5743198278272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.841942e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.241975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-1.85295636641172;-8.25338563527774;-9.66795474465834]
Robot Arm 1 end: [-8.30095962569269;-10.1709529410753;0.761730412330003]
Robot Arm 1 togoal: 27.2464485039623
Mobile Platform 2 Pose: [0.294741828970998;8.08580046564115;7.42676718381734]
Robot Arm 2 end: [8.22627499623278;7.78664677156711;0.156465375197621]
Robot Arm 1 togoal: 25.5034087595577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.218653e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-1.8531851911903;-8.24652875163986;-9.66637545418586]
Robot Arm 1 end: [-7.98027474966297;-9.99831396462247;1.04541511634395]
Robot Arm 1 togoal: 26.8970076864053
Mobile Platform 2 Pose: [0.288484019058373;8.0872349560627;7.42488984084356]
Robot Arm 2 end: [7.53822800042297;6.96261058355045;0.895171744587108]
Robot Arm 1 togoal: 24.4071968937293

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.267004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.427858e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-1.85318519115845;-8.24652875176504;-9.66637545430163]
Robot Arm 1 end: [-7.91034011161679;-9.87692686564445;1.10497892205556]
Robot Arm 1 togoal: 26.7590279732516
Mobile Platform 2 Pose: [0.288484021384883;8.0872349545296;7.42488984048178]
Robot Arm 2 end: [7.57742286904432;6.90642399279846;0.945515246188642]
Robot Arm 1 togoal: 24.3951665221483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.602937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.055907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-1.87588570500529;-8.19144000018136;-9.63400488579475]
Robot Arm 1 end: [-8.22085682169758;-9.8440901017567;1.39028869644279]
Robot Arm 1 togoal: 26.940761324215
Mobile Platform 2 Pose: [0.28848402218359;8.08723495430834;7.424889840577]
Robot Arm 2 end: [7.47109453994719;7.03843439553862;0.990207542218463]
Robot Arm 1 togoal: 24.4096114616371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989173e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.937625e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-1.87588570500532;-8.19144000018134;-9.63400488579476]
Robot Arm 1 end: [-8.30889077210245;-9.66751815431697;1.38503822869333]
Robot Arm 1 togoal: 26.8708994501193
Mobile Platform 2 Pose: [0.288484022183591;8.08723495430834;7.424889840577]
Robot Arm 2 end: [7.43851103494397;7.12568667754701;1.03834478163206]
Robot Arm 1 togoal: 24.4463446624069

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.637866e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-1.87588570500532;-8.19144000018134;-9.63400488579476]
Robot Arm 1 end: [-8.26849285794276;-9.56294351105347;1.39601058790326]
Robot Arm 1 togoal: 26.7668117726133
Mobile Platform 2 Pose: [0.288484018990563;8.0872349570467;7.42488984084769]
Robot Arm 2 end: [7.60796055800394;7.03644775167933;1.18190403667556]
Robot Arm 1 togoal: 24.5029755964508

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.816670e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.322225e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-1.87588570378777;-8.19144000124292;-9.63400488600744]
Robot Arm 1 end: [-8.17345336040086;-9.42328884435616;1.40011820496329]
Robot Arm 1 togoal: 26.5998715971023
Mobile Platform 2 Pose: [0.288484024837673;8.08723495228509;7.42488983997878]
Robot Arm 2 end: [7.7896762804464;7.01682588060695;1.28244467756004]
Robot Arm 1 togoal: 24.6191359282944

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.049152e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.175858e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-2.12188402377536;-8.03734449261652;-9.6362051431895]
Robot Arm 1 end: [-7.42675605137132;-9.66116605196328;0.97676554067161]
Robot Arm 1 togoal: 26.2783079391273
Mobile Platform 2 Pose: [0.288484024837673;8.08723495228509;7.42488983997878]
Robot Arm 2 end: [7.8865493069112;6.82467909533837;1.14084885174222]
Robot Arm 1 togoal: 24.5589695969762

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.950654e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.557829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-2.1218840249815;-8.03734449156637;-9.63620514344644]
Robot Arm 1 end: [-7.39094276125757;-9.71088874884534;0.936538573799147]
Robot Arm 1 togoal: 26.2926913904494
Mobile Platform 2 Pose: [-0.294424979714673;8.20000876533369;7.42298584604785]
Robot Arm 2 end: [7.83401683580894;6.73447165552862;0.940744079005888]
Robot Arm 1 togoal: 24.4628574582663

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.910023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.559567e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-2.25084697689317;-7.94556791009672;-9.63519230963333]
Robot Arm 1 end: [-7.40265053310435;-9.07591630387878;0.748732863178048]
Robot Arm 1 togoal: 25.8329097265593
Mobile Platform 2 Pose: [-0.294424979454227;8.20000876514018;7.42298584598456]
Robot Arm 2 end: [7.72018016420423;6.89093911482239;1.09910864695394]
Robot Arm 1 togoal: 24.4844154830429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.469825e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.573222e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-2.25084697689317;-7.94556791009672;-9.63519230963333]
Robot Arm 1 end: [-7.29579619992472;-9.18411813562461;0.527034259860764]
Robot Arm 1 togoal: 25.8488476091972
Mobile Platform 2 Pose: [-0.052410463598838;8.00639715245587;7.35038149122794]
Robot Arm 2 end: [8.57698568024975;7.66130349382245;1.16410206370891]
Robot Arm 1 togoal: 25.6350036903067

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.370102e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.036854e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-2.95832023705341;-7.44719143049714;-9.5997270268819]
Robot Arm 1 end: [-7.23146925005859;-8.88531391200049;0.889899192756231]
Robot Arm 1 togoal: 25.5729721053189
Mobile Platform 2 Pose: [-0.052410465278461;8.00639715289967;7.35038149063909]
Robot Arm 2 end: [8.52645028787521;7.85651092862582;0.999152162799663]
Robot Arm 1 togoal: 25.7362908745304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.138253e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.091451e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-3.25236849974938;-7.26335720710893;-9.5106223446016]
Robot Arm 1 end: [-7.28654979555799;-9.70865279287226;-0.15043947925427]
Robot Arm 1 togoal: 26.2687934377867
Mobile Platform 2 Pose: [-0.0727387961090693;8.02265981756415;7.3442829913899]
Robot Arm 2 end: [7.38826083503878;7.76267471934815;0.860444962839739]
Robot Arm 1 togoal: 24.8656502229447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.687803e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.492635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-3.23587092654763;-7.27723059329401;-9.49316709685667]
Robot Arm 1 end: [-7.65598759450257;-8.75503318789763;0.721004260789296]
Robot Arm 1 togoal: 25.770625933398
Mobile Platform 2 Pose: [-0.0357132322587621;8.00729358528118;7.33448736949318]
Robot Arm 2 end: [7.8675119784371;7.68825018666114;1.34443310395664]
Robot Arm 1 togoal: 25.142660058838

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.192972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.485804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-3.28006742792295;-7.25256864480641;-9.49111546673903]
Robot Arm 1 end: [-7.54442695723119;-8.89726043184706;0.275434309025927]
Robot Arm 1 togoal: 25.8159796052213
Mobile Platform 2 Pose: [-0.0388639678377723;8.01049348728987;7.3266070652515]
Robot Arm 2 end: [8.15066041073527;7.23220374578961;1.39912524520628]
Robot Arm 1 togoal: 25.0281875639866

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.666055e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.758302e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-3.28006742792295;-7.25256864480641;-9.49111546673903]
Robot Arm 1 end: [-7.37607632719383;-8.87511069617972;0.215235967799753]
Robot Arm 1 togoal: 25.6885743479332
Mobile Platform 2 Pose: [-0.0905806719893307;8.04489442752194;7.31812929477965]
Robot Arm 2 end: [8.27897943194228;6.69015240953653;1.04273330729063]
Robot Arm 1 togoal: 24.7570397171447

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.730227e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.046538e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-3.28006742818206;-7.25256864442701;-9.49111546680268]
Robot Arm 1 end: [-7.41519423195978;-8.93776553801491;0.157581079585087]
Robot Arm 1 togoal: 25.7640614977968
Mobile Platform 2 Pose: [-0.0905806728752462;8.04489442813154;7.3181292947061]
Robot Arm 2 end: [8.31233988600042;6.65797033522798;0.943856559838596]
Robot Arm 1 togoal: 24.7621255618386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.028772e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.567587e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-3.28006742818206;-7.25256864442701;-9.49111546680268]
Robot Arm 1 end: [-7.23708116805842;-8.98232891031802;0.107522643805075]
Robot Arm 1 togoal: 25.6796358286134
Mobile Platform 2 Pose: [-0.0976685768227463;8.04631200892104;7.31600292352185]
Robot Arm 2 end: [7.93745258007053;7.80274286644584;1.27167748792354]
Robot Arm 1 togoal: 25.2735438818163

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.581638e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.164916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-3.28006742818206;-7.25256864442701;-9.49111546680268]
Robot Arm 1 end: [-7.05036388701318;-8.92022553702041;0.270425942540326]
Robot Arm 1 togoal: 25.5000740886075
Mobile Platform 2 Pose: [-0.0976685768227463;8.04631200892104;7.31600292352185]
Robot Arm 2 end: [7.92023247989111;7.65899538746553;1.38153392334063]
Robot Arm 1 togoal: 25.1593779933927

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.895722e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.731635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-3.28006742818206;-7.25256864442701;-9.49111546680268]
Robot Arm 1 end: [-6.84632885932628;-8.97472349576862;0.279709788688148]
Robot Arm 1 togoal: 25.4043250090637
Mobile Platform 2 Pose: [-0.0976685774565133;8.04631200949981;7.31600292346811]
Robot Arm 2 end: [7.86714192633802;7.56027190907015;1.38760979466769]
Robot Arm 1 togoal: 25.0522594406825

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.258926e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.341439e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-3.28006742737369;-7.25256864463568;-9.49111546666049]
Robot Arm 1 end: [-6.69783269789414;-9.08351329342649;0.293854672650586]
Robot Arm 1 togoal: 25.3870829770118
Mobile Platform 2 Pose: [-0.097668576829802;8.04631200913949;7.31600292371023]
Robot Arm 2 end: [7.88014358978987;7.46734528986589;1.38497553377659]
Robot Arm 1 togoal: 24.9965235367304

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.164455e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.630526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-3.28006742750999;-7.2525686446968;-9.49111546678976]
Robot Arm 1 end: [-6.63352244734375;-9.09140823035557;0.395390713733855]
Robot Arm 1 togoal: 25.3460634712711
Mobile Platform 2 Pose: [-0.0976685768298019;8.04631200913949;7.31600292371023]
Robot Arm 2 end: [7.68250446586628;7.49726051407254;1.31977547762504]
Robot Arm 1 togoal: 24.8770090845163

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.080375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.281972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-3.28006742782399;-7.25256864462267;-9.49111546691594]
Robot Arm 1 end: [-6.75369430349087;-8.91474516095768;0.510490538111744]
Robot Arm 1 togoal: 25.2878426658219
Mobile Platform 2 Pose: [-0.0976685771079376;8.04631200926969;7.31600292376579]
Robot Arm 2 end: [7.50955016966795;7.42563876935722;1.22643972863576]
Robot Arm 1 togoal: 24.7047363762035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.055332e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.027879e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-3.28006743203713;-7.25256864194258;-9.49111546613492]
Robot Arm 1 end: [-6.92508210714828;-8.79945093824975;0.621045487552172]
Robot Arm 1 togoal: 25.3118090629225
Mobile Platform 2 Pose: [-0.0976685768018331;8.04631200858842;7.31600292421558]
Robot Arm 2 end: [7.39687473457113;7.29405875264222;1.12662544512204]
Robot Arm 1 togoal: 24.5334720323018

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.400207e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.247432e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-4.28853905362036;-6.66021590900092;-9.43260746553475]
Robot Arm 1 end: [-7.02603881547954;-9.66857391904477;-0.0347184864118193]
Robot Arm 1 togoal: 26.0606803012528
Mobile Platform 2 Pose: [0.136154449576789;7.85925358748552;7.25633543217708]
Robot Arm 2 end: [7.41153362160021;7.1311108928011;1.30431090348867]
Robot Arm 1 togoal: 24.4271041599718

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.124812e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.662572e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-4.30965602605001;-6.65296842629571;-9.42486922503946]
Robot Arm 1 end: [-6.42801045459676;-9.48980466541455;0.042351961875165]
Robot Arm 1 togoal: 25.5326930309555
Mobile Platform 2 Pose: [0.136154450331402;7.85925358638688;7.2563354322859]
Robot Arm 2 end: [7.31988226764234;7.0922978748127;1.20257669127068]
Robot Arm 1 togoal: 24.3357013831502

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.032816e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.438594e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-4.30965602614013;-6.65296842622362;-9.4248692250205]
Robot Arm 1 end: [-6.43709575501696;-9.39522543618735;0.00724911750391749]
Robot Arm 1 togoal: 25.4684967816003
Mobile Platform 2 Pose: [0.136154450331402;7.85925358638688;7.2563354322859]
Robot Arm 2 end: [7.21771353054785;6.93587294574349;0.274144730929193]
Robot Arm 1 togoal: 24.1832193827949

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.301815e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.279857e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-4.30965602583245;-6.65296842562986;-9.42486922477331]
Robot Arm 1 end: [-6.4857958248662;-9.22598661367662;-0.0154028487378856]
Robot Arm 1 togoal: 25.3728311183845
Mobile Platform 2 Pose: [0.136154451029369;7.85925358582851;7.25633543215745]
Robot Arm 2 end: [7.21808130213248;6.8203266369528;0.46890255718912]
Robot Arm 1 togoal: 24.0934144722121

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.584757e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.000973e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-4.30965602583245;-6.65296842562986;-9.42486922477331]
Robot Arm 1 end: [-6.60028115709913;-9.03476929846691;0.0185326557788063]
Robot Arm 1 togoal: 25.3011696342491
Mobile Platform 2 Pose: [0.13615445006571;7.85925358629988;7.25633543208131]
Robot Arm 2 end: [7.12855058986386;6.98787678605647;0.678507129613256]
Robot Arm 1 togoal: 24.1388690133654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.364097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.415867e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-4.16819126795651;-6.73608883910855;-9.35296562456724]
Robot Arm 1 end: [-6.61274581663804;-9.67360091789879;1.07083409258622]
Robot Arm 1 togoal: 25.7533793120276
Mobile Platform 2 Pose: [0.430782276936177;7.62355132535117;7.16794708329672]
Robot Arm 2 end: [6.90127644419126;7.10948634354416;0.86684669562116]
Robot Arm 1 togoal: 24.0585865114418

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.524520e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.833706e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-4.12672254689057;-6.76336215609;-9.33591310493738]
Robot Arm 1 end: [-6.94880569218917;-9.25209016628957;1.17888675515046]
Robot Arm 1 togoal: 25.6519401144403
Mobile Platform 2 Pose: [1.02003792813447;7.32892349976561;6.99117038793724]
Robot Arm 2 end: [6.59531538328728;7.02762384940549;0.717331044802143]
Robot Arm 1 togoal: 23.7905517047664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.401183e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.651316e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-4.12672254801792;-6.76336215551512;-9.33591310435586]
Robot Arm 1 end: [-7.10703336062538;-9.16048542472703;1.12137387078214]
Robot Arm 1 togoal: 25.6891902004691
Mobile Platform 2 Pose: [1.02003792528342;7.3289235006617;6.99117038852013]
Robot Arm 2 end: [6.60735620029694;6.95835645134922;0.573551855771409]
Robot Arm 1 togoal: 23.7547425050359

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.726934e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.655397e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-4.34631844796021;-6.6446343306926;-9.29664218944346]
Robot Arm 1 end: [-6.66557796347167;-9.10841328967735;-0.142685026983944]
Robot Arm 1 togoal: 25.4094627092877
Mobile Platform 2 Pose: [1.37046315079975;7.22070329803164;6.88604282086523]
Robot Arm 2 end: [6.51913602874782;6.77718564358798;0.453120276738083]
Robot Arm 1 togoal: 23.5689664677737

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.520860e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.860888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-4.35735916755078;-6.63828997721519;-9.28445603995521]
Robot Arm 1 end: [-6.66340711583491;-8.56687178526533;0.665346072566191]
Robot Arm 1 togoal: 24.9625721479593
Mobile Platform 2 Pose: [1.38170062668688;7.21328984741166;6.8856461932022]
Robot Arm 2 end: [6.58889429904351;6.99469355846254;0.353247292102373]
Robot Arm 1 togoal: 23.7776085760887

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.993143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.719845e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-4.22457213145011;-6.71511620159003;-9.27552845713298]
Robot Arm 1 end: [-6.96971735633713;-8.77689847651699;1.13781214489909]
Robot Arm 1 togoal: 25.3118560458077
Mobile Platform 2 Pose: [1.38170062668688;7.21328984741166;6.8856461932022]
Robot Arm 2 end: [6.52357371627344;6.8830771953288;0.521378985724861]
Robot Arm 1 togoal: 23.6445770973544

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.817139e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.736305e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-4.21555758025213;-6.73591464086483;-9.2234714726599]
Robot Arm 1 end: [-6.39675463977359;-8.89826880227259;-0.0298454438124823]
Robot Arm 1 togoal: 25.0679725552443
Mobile Platform 2 Pose: [1.38170062668688;7.21328984741166;6.8856461932022]
Robot Arm 2 end: [6.49809004835135;6.66772421216241;0.648946563045693]
Robot Arm 1 togoal: 23.4682867472919

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.040777e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.032805e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-4.04620760738994;-6.83369105780313;-9.21038026216517]
Robot Arm 1 end: [-7.03072184952439;-8.52908784915835;0.912993513871007]
Robot Arm 1 togoal: 25.1742379254044
Mobile Platform 2 Pose: [1.38170062690474;7.21328984736559;6.88564619284354]
Robot Arm 2 end: [6.56277138514384;6.46838860418409;0.789987801556672]
Robot Arm 1 togoal: 23.3680893101946

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.190526e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.375321e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-4.04620760560196;-6.8336910591297;-9.21038026192245]
Robot Arm 1 end: [-6.93965028143986;-8.61797769079239;1.11617453840203]
Robot Arm 1 togoal: 25.1742870525997
Mobile Platform 2 Pose: [1.38170062424238;7.213289849025;6.88564619329399]
Robot Arm 2 end: [6.69406692112702;6.31638396040004;0.921274481224605]
Robot Arm 1 togoal: 23.3511440106933

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.940389e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.627415e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-3.87831769079306;-6.93391742780551;-9.19322725886593]
Robot Arm 1 end: [-6.55992034738509;-8.9125445576858;1.30854974712309]
Robot Arm 1 togoal: 25.138823334996
Mobile Platform 2 Pose: [1.674303492306;6.97920755457411;6.79786533287491]
Robot Arm 2 end: [7.27908093881612;7.400774448241;0.19878474886675]
Robot Arm 1 togoal: 24.5581319049118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.903580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.297726e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-3.87831769079306;-6.93391742780551;-9.19322725886593]
Robot Arm 1 end: [-6.70639068243835;-8.85774142413485;1.37303416908028]
Robot Arm 1 togoal: 25.1940428502635
Mobile Platform 2 Pose: [1.6743034921862;6.9792075547971;6.79786533283897]
Robot Arm 2 end: [7.19287253278422;7.44589334517075;0.257759554958262]
Robot Arm 1 togoal: 24.5265129392996

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.816563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.076529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-3.60743273318638;-7.07963849747945;-9.11666995321514]
Robot Arm 1 end: [-7.34239763347789;-9.39137185395421;0.117355896826259]
Robot Arm 1 togoal: 26.0529129815528
Mobile Platform 2 Pose: [1.6743034921862;6.9792075547971;6.79786533283897]
Robot Arm 2 end: [7.02768989503923;7.47132691927667;0.20160143598794]
Robot Arm 1 togoal: 24.4321570896346

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.421264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-3.59281600633653;-7.09253613484434;-9.11726441820658]
Robot Arm 1 end: [-7.76996459241503;-9.07948763539699;0.267534193947149]
Robot Arm 1 togoal: 26.1030585673766
Mobile Platform 2 Pose: [1.67430349212825;6.97920755484347;6.79786533282158]
Robot Arm 2 end: [6.85127050739152;7.41877972874426;0.193549614841267]
Robot Arm 1 togoal: 24.2722337142022

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.059453e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-3.59281600633653;-7.09253613484434;-9.11726441820658]
Robot Arm 1 end: [-7.84967324127395;-8.96284636928528;0.445534977722079]
Robot Arm 1 togoal: 26.0644777860503
Mobile Platform 2 Pose: [1.67430349212825;6.97920755484347;6.79786533282158]
Robot Arm 2 end: [6.68870421518579;7.32744055522994;0.2117697800155]
Robot Arm 1 togoal: 24.0928854300581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.134760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.236771e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-3.59281600596118;-7.09253613508431;-9.11726441858546]
Robot Arm 1 end: [-7.85941345717287;-8.82065374798794;0.675077718876639]
Robot Arm 1 togoal: 25.9594152677136
Mobile Platform 2 Pose: [1.67430349212825;6.97920755484347;6.79786533282158]
Robot Arm 2 end: [6.53818502508179;7.22186783896178;0.214784027656333]
Robot Arm 1 togoal: 23.9125593768821

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.891640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.372820e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-3.74494607756728;-7.01545106722069;-9.10077276535352]
Robot Arm 1 end: [-7.37484824608868;-8.95015070861951;1.36666166343241]
Robot Arm 1 togoal: 25.7102583374057
Mobile Platform 2 Pose: [1.67430349166634;6.97920755509131;6.79786533281289]
Robot Arm 2 end: [6.41439044025221;7.11316129891283;0.385944429879722]
Robot Arm 1 togoal: 23.7398579253312

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.250660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.948432e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-4.11383006526787;-6.76307176169729;-9.12624403542965]
Robot Arm 1 end: [-6.92084214290942;-9.63358992745457;0.904106487937823]
Robot Arm 1 togoal: 25.9263053648207
Mobile Platform 2 Pose: [1.67430349165376;6.97920755505446;6.7978653329661]
Robot Arm 2 end: [6.40282096710271;6.97334790098107;0.344361758095427]
Robot Arm 1 togoal: 23.6332721432914

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.044824e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-4.11431244966943;-6.76385001630112;-9.12603141535331]
Robot Arm 1 end: [-6.86975759468913;-9.66103315726081;0.765964078963466]
Robot Arm 1 togoal: 25.9174656430087
Mobile Platform 2 Pose: [1.67430349165376;6.97920755505446;6.7978653329661]
Robot Arm 2 end: [6.45001305310941;6.87282764273922;0.159427550160329]
Robot Arm 1 togoal: 23.604006474436

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.018467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.378413e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-4.12143048208631;-6.75530808303908;-9.11345111619978]
Robot Arm 1 end: [-6.87972329265414;-8.94651433395834;1.08916812189075]
Robot Arm 1 togoal: 25.3787695252235
Mobile Platform 2 Pose: [1.67430349049409;6.97920755628331;6.79786533293951]
Robot Arm 2 end: [6.35737769972562;6.85486929034163;0.293352475525133]
Robot Arm 1 togoal: 23.5193014872962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.719193e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.359617e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-4.15732460610049;-6.73391626374225;-9.10635789849776]
Robot Arm 1 end: [-6.82649055917551;-8.6895765894247;1.24297646144779]
Robot Arm 1 togoal: 25.1497187800974
Mobile Platform 2 Pose: [1.67430349049409;6.97920755628331;6.79786533293951]
Robot Arm 2 end: [6.34639559901443;6.80987555059639;0.236970567894482]
Robot Arm 1 togoal: 23.4824465652225

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.711664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.958917e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-4.15732460610049;-6.73391626374225;-9.10635789849776]
Robot Arm 1 end: [-6.70128976077513;-8.74647993555843;1.22238796817053]
Robot Arm 1 togoal: 25.1088183472106
Mobile Platform 2 Pose: [1.6743034898063;6.97920755643831;6.7978653332842]
Robot Arm 2 end: [6.2969105376022;6.72606706832229;0.37531264461522]
Robot Arm 1 togoal: 23.380830014371
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 90)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',90,0)">第 90 行</a>)
    real_time_2D_plot(human_poses{k-1}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.556482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.259141566892;-10.0035524791063;1.47381471675359]
Robot Arm 1 togoal: 28.4706367203596
Mobile Platform 2 Pose: [-2.35426302645963;9.99835457200422;8.17368709105705]
Robot Arm 2 end: [10.4656142450962;8.1544767461757;1.33377235157311]
Robot Arm 1 togoal: 27.3580234400962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.072178e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785398163816203;-10.0000000004024;-9.99999999998701]
Robot Arm 1 end: [-10.4383911944831;-9.89000075687976;1.3825988437274]
Robot Arm 1 togoal: 28.5194516007489
Mobile Platform 2 Pose: [-2.19434433851679;9.88812478838991;8.68065201444096]
Robot Arm 2 end: [10.4149852333523;8.41263586301914;0.100805323835227]
Robot Arm 1 togoal: 27.5284168527785

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.037785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.480438470987;-9.87299582418333;1.36188030313921]
Robot Arm 1 togoal: 28.5378633059629
Mobile Platform 2 Pose: [-2.32896808248348;9.9603306751678;9.05360996406699]
Robot Arm 2 end: [10.6670070938668;8.71194456717928;0.954648133449663]
Robot Arm 1 togoal: 27.8851717968506

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.719516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785398163542596;-10.0000000000439;-9.99999999998629]
Robot Arm 1 end: [-10.5056015524634;-9.79946133438362;1.31710470099772]
Robot Arm 1 togoal: 28.5050861681401
Mobile Platform 2 Pose: [-2.41948243274282;10.0130745871473;9.34677566828691]
Robot Arm 2 end: [10.1607579902366;9.26146880224983;1.49290831508202]
Robot Arm 1 togoal: 27.8829900172792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.856737e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.4258244916062;-9.7068942851601;1.30974479787038]
Robot Arm 1 togoal: 28.3834567536938
Mobile Platform 2 Pose: [-1.96423084560555;9.66375273817669;9.6643302847348]
Robot Arm 2 end: [9.37159645367595;10.0662358991674;0.684996892366847]
Robot Arm 1 togoal: 27.9036104029761

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.617872e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.2905006738355;-9.81246969326846;1.4170519057284]
Robot Arm 1 togoal: 28.3592873349791
Mobile Platform 2 Pose: [-1.98571163013661;9.71920759263834;9.88483978036536]
Robot Arm 2 end: [9.44641997664344;9.88369991894267;0.0948133473130907]
Robot Arm 1 togoal: 27.8488560042035

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.124851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.78539816440861;-10.0000000005907;-9.99999999998682]
Robot Arm 1 end: [-10.2757265175814;-9.93179103862621;1.46568108554116]
Robot Arm 1 togoal: 28.432102279035
Mobile Platform 2 Pose: [-2.20248089881646;9.8945678267457;9.96815145955904]
Robot Arm 2 end: [9.66271646190082;9.51181028594518;0.682747294842516]
Robot Arm 1 togoal: 27.7134593093956

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.117629e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.1370202992013;-9.97274463497696;1.50568202970272]
Robot Arm 1 togoal: 28.3621287886622
Mobile Platform 2 Pose: [-2.35619449294359;10.000000002414;10.0000000027873]
Robot Arm 2 end: [9.64771673252946;9.57139396093481;0.500654394097941]
Robot Arm 1 togoal: 27.7509160124728

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.357009e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.0551559445985;-10.0012133461152;1.519169546625]
Robot Arm 1 togoal: 28.324156097394
Mobile Platform 2 Pose: [-2.30625672595817;9.95339903206351;9.99026375342538]
Robot Arm 2 end: [9.74264083983572;10.1122897455876;-0.0960017298675314]
Robot Arm 1 togoal: 28.2292536340515

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.545326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785398160406159;-9.9999999980484;-10.000000000492]
Robot Arm 1 end: [-10.1577853159938;-10.0360315719418;1.50027934455254]
Robot Arm 1 togoal: 28.4214584309656
Mobile Platform 2 Pose: [-2.35619448959639;9.99999999967847;10.0000000000874]
Robot Arm 2 end: [9.6260056809909;9.99411237829594;-0.0378254496978475]
Robot Arm 1 togoal: 28.0601953943453

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.959755e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785398164306627;-10.0000000008884;-10.000000000539]
Robot Arm 1 end: [-10.1903456929746;-10.1074784169038;1.47938599527051]
Robot Arm 1 togoal: 28.4949553292062
Mobile Platform 2 Pose: [-2.35619449019235;10;10]
Robot Arm 2 end: [9.54331893104006;9.79319821068108;0.065715150711132]
Robot Arm 1 togoal: 27.8537226280479

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.160049e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.2626524681405;-10.0816342379948;1.4379545688129]
Robot Arm 1 togoal: 28.5281267393582
Mobile Platform 2 Pose: [-2.19134150322778;9.87949556136343;9.99021727001915]
Robot Arm 2 end: [9.52291695746268;9.96370618111985;1.28658366500252]
Robot Arm 1 togoal: 27.923982560376

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.243736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.78539816322453;-9.99999999986166;-9.99999999994813]
Robot Arm 1 end: [-10.16799961524;-10.1397942345808;1.47939727254454]
Robot Arm 1 togoal: 28.5019517064798
Mobile Platform 2 Pose: [-2.35619449441206;10.0000000032567;10.0000000002335]
Robot Arm 2 end: [9.52248611707164;9.81812061121994;1.19306846440005]
Robot Arm 1 togoal: 27.8207280250564

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.2360643918858;-10.1388485920794;1.43825843595122]
Robot Arm 1 togoal: 28.5495774198791
Mobile Platform 2 Pose: [-2.17751032641694;9.87235722674737;9.99011281989408]
Robot Arm 2 end: [10.4545254759428;9.87934845386663;-0.00629313853565688]
Robot Arm 1 togoal: 28.5641161551913

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.544054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.1333955561188;-10.1731007678077;1.47940058902255]
Robot Arm 1 togoal: 28.5010410637623
Mobile Platform 2 Pose: [-2.32693365558563;9.96221138205438;9.98510186123426]
Robot Arm 2 end: [9.2274866204929;10.0051198340843;0.491846633402523]
Robot Arm 1 togoal: 27.7661703871688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.279680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785398162891652;-9.99999999959536;-10.0000000001517]
Robot Arm 1 end: [-10.1701152267314;-10.2289705826261;1.43425527220641]
Robot Arm 1 togoal: 28.5666295481314
Mobile Platform 2 Pose: [-2.1585268480974;9.87609226035162;9.99010842625204]
Robot Arm 2 end: [9.86648989546539;9.71558760907871;-0.0976187125455996]
Robot Arm 1 togoal: 28.0357226066048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.402413e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.91744549114871;-10.1437431568604;1.42892543741756]
Robot Arm 1 togoal: 28.3281397375033
Mobile Platform 2 Pose: [-2.35619449075891;10.0000000005073;9.99999999837774]
Robot Arm 2 end: [9.76046473819097;9.71589560870661;-0.0439275485278274]
Robot Arm 1 togoal: 27.9578522879503

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.311190e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785398163865563;-10.0000000004417;-9.99999999982335]
Robot Arm 1 end: [-9.99184044898549;-10.3197357554521;1.3870737104134]
Robot Arm 1 togoal: 28.5058464522654
Mobile Platform 2 Pose: [-2.29101414964761;9.95248021540466;9.98776017640179]
Robot Arm 2 end: [9.29668689460883;9.94224027921546;0.975865916531827]
Robot Arm 1 togoal: 27.755220114989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.198540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785398164545579;-10.0000000009392;-10.0000000000623]
Robot Arm 1 end: [-9.92622305721069;-10.2849503605123;1.43612003003549]
Robot Arm 1 togoal: 28.4348515217411
Mobile Platform 2 Pose: [-2.35619449077571;10.0000000004868;9.99999999904236]
Robot Arm 2 end: [9.2891437729744;9.74466062856938;0.786967614935247]
Robot Arm 1 togoal: 27.6127053070535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.717087e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785398162450288;-9.99999999936124;-10.0000000000729]
Robot Arm 1 end: [-9.78282132409714;-10.1460635713861;1.44243374312899]
Robot Arm 1 togoal: 28.2352628749973
Mobile Platform 2 Pose: [-2.04953756352443;9.75467445866567;9.94220113747122]
Robot Arm 2 end: [10.2675224295226;10.3970722159717;1.1331459834515]
Robot Arm 1 togoal: 28.756972218106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.997554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.7853981626984;-9.99999999923836;-9.99999999987156]
Robot Arm 1 end: [-9.7987294389733;-10.0368176917526;1.46464314092273]
Robot Arm 1 togoal: 28.1685449873749
Mobile Platform 2 Pose: [-2.25303610738777;9.91952208045656;9.98213662400797]
Robot Arm 2 end: [10.0662910934574;10.2553290855042;1.39450827314001]
Robot Arm 1 togoal: 28.5122849797487

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.598770e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.70215592649089;-10.1559563081784;1.41418834497951]
Robot Arm 1 togoal: 28.1859704743087
Mobile Platform 2 Pose: [-2.35619448871253;9.99999999899266;9.99999999956285]
Robot Arm 2 end: [9.95851305532042;10.2273337391317;1.43883120432427]
Robot Arm 1 togoal: 28.4164393491618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.018790e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.78539816301582;-9.99999999971595;-10.0000000004201]
Robot Arm 1 end: [-9.77410355979988;-10.020418167922;1.45792465670346]
Robot Arm 1 togoal: 28.139585883788
Mobile Platform 2 Pose: [-2.35619449446023;10.0000000039152;10.0000000001935]
Robot Arm 2 end: [9.81038770569707;10.076202514773;1.44975295113575]
Robot Arm 1 togoal: 28.2047947095203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.459128e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785398164166827;-10.0000000005336;-10.0000000002061]
Robot Arm 1 end: [-9.77908088189064;-10.1252384373505;1.47461855862442]
Robot Arm 1 togoal: 28.2177145412361
Mobile Platform 2 Pose: [-2.35619448176896;9.9999999941096;9.99999999975896]
Robot Arm 2 end: [9.73177233822838;9.8605544332676;1.41021706573835]
Robot Arm 1 togoal: 27.9963700301125

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.778850e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785398156830157;-9.99999999419811;-10.0000000023007]
Robot Arm 1 end: [-9.91017319379221;-10.0098203649742;1.50422591560185]
Robot Arm 1 togoal: 28.2277910757332
Mobile Platform 2 Pose: [-1.7669388314214;9.52859547298324;10.0000000007014]
Robot Arm 2 end: [9.33248471740326;10.0757997390976;1.44683798356761]
Robot Arm 1 togoal: 27.8709203385476

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.942772e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785398163369359;-9.99999999972753;-10.0000000000896]
Robot Arm 1 end: [-9.75924159049472;-10.0322104884664;1.47814448532812]
Robot Arm 1 togoal: 28.1375002777591
Mobile Platform 2 Pose: [-1.74555941753933;9.51149194187759;9.94332012516678]
Robot Arm 2 end: [9.26253994571584;9.25980028387083;1.03352313264378]
Robot Arm 1 togoal: 27.2437696699832

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.141916e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.71332279235046;-10.218828231991;1.43073995155444]
Robot Arm 1 togoal: 28.2387014125385
Mobile Platform 2 Pose: [-2.25103832130486;9.91587506489001;9.96845314933376]
Robot Arm 2 end: [9.59903073568513;9.44103633421511;1.19411882288956]
Robot Arm 1 togoal: 27.607657467278

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.522150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785398164864108;-10.0000000011733;-10.000000001131]
Robot Arm 1 end: [-9.72625685202973;-10.0630108298745;1.46618559001454]
Robot Arm 1 togoal: 28.1363220334758
Mobile Platform 2 Pose: [-1.64677622838249;9.51475485625299;9.7529421490286]
Robot Arm 2 end: [9.43907753144561;9.72418904533536;0.568371881234876]
Robot Arm 1 togoal: 27.7097251719021

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.694137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785398161029181;-9.99999999817909;-10.0000000004199]
Robot Arm 1 end: [-9.71332278084681;-10.2188282386222;1.430739947029]
Robot Arm 1 togoal: 28.2387014092702
Mobile Platform 2 Pose: [-2.08185561300285;9.78052889846703;9.91769833554357]
Robot Arm 2 end: [9.64265241223717;9.83750870888503;0.532753309709284]
Robot Arm 1 togoal: 27.9345158734481

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.968552e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.93137655865621;-10.1568293264023;1.4779052332981]
Robot Arm 1 togoal: 28.3471232610221
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.8991244149762;9.82635089999213;0.533478062927636]
Robot Arm 1 togoal: 28.1075531142168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.754296e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785398164185405;-10.0000000002399;-9.99999999978146]
Robot Arm 1 end: [-9.94754100617866;-10.3175644780202;1.40997390442933]
Robot Arm 1 togoal: 28.4731477941162
Mobile Platform 2 Pose: [-2.28763393028516;9.95016750366993;9.98764368097137]
Robot Arm 2 end: [9.80053182753225;9.38119436912186;1.02451502933739]
Robot Arm 1 togoal: 27.7116988499964

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.490926e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.77403039190568;-10.4119874860751;1.35455849409212]
Robot Arm 1 togoal: 28.4198735719669
Mobile Platform 2 Pose: [-1.87814531914992;9.7372663662084;9.85658524868728]
Robot Arm 2 end: [10.0297555364157;9.18534365856173;0.312645509585029]
Robot Arm 1 togoal: 27.7619999049095

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.299743e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.85924570522429;-10.5759035043233;1.22581514667201]
Robot Arm 1 togoal: 28.5979820808624
Mobile Platform 2 Pose: [-2.2257008227696;9.92548980794039;9.95437955705946]
Robot Arm 2 end: [10.4764114694864;9.56870221297732;0.356889146677434]
Robot Arm 1 togoal: 28.347357673101

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.359213e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.548579e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785398163477385;-10.0000000000733;-9.99999999991565]
Robot Arm 1 end: [-9.9772128421247;-10.6967479905902;1.04681756407631]
Robot Arm 1 togoal: 28.7692415490588
Mobile Platform 2 Pose: [-2.334324765856;9.98721753242196;9.983787935378]
Robot Arm 2 end: [10.4569960149753;9.52657826948325;0.288813389298241]
Robot Arm 1 togoal: 28.3071409491415

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.937269e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.721300e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.2369384392669;-10.0465713666129;0.0042897292368968]
Robot Arm 1 togoal: 28.5254116709438
Mobile Platform 2 Pose: [-2.40340983550027;10.0250519409569;9.98547400365829]
Robot Arm 2 end: [10.5053320247338;10.37756970591;0.0885131682733233]
Robot Arm 1 togoal: 28.9441877945619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.881016e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.471895e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.785398163242959;-9.99999999972302;-10.0000000002507]
Robot Arm 1 end: [-10.231728238247;-10.0300230060914;0.0424819006936176]
Robot Arm 1 togoal: 28.5080806947926
Mobile Platform 2 Pose: [-2.35619449041414;10.0000000001731;9.99999999961752]
Robot Arm 2 end: [10.4977335001572;10.5143131760371;0.26946882870954]
Robot Arm 1 togoal: 29.0268919107904

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.360625e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.187213e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785398164615546;-10.0000000008612;-9.99999999993132]
Robot Arm 1 end: [-10.2191153518394;-10.117937534463;0.0239977550365835]
Robot Arm 1 togoal: 28.5619511233559
Mobile Platform 2 Pose: [-2.261963245757;9.9470038880497;10.0050368319116]
Robot Arm 2 end: [9.85847924585232;10.6926231634267;0.169698140466364]
Robot Arm 1 togoal: 28.7118419481772

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.334648e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.058948e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.1544284177739;-10.2959029154881;0.0432716116547685]
Robot Arm 1 togoal: 28.6410254912741
Mobile Platform 2 Pose: [-2.33866790396102;9.99061646867671;9.99816285549307]
Robot Arm 2 end: [10.0564516296886;10.6616768491873;0.154952536305256]
Robot Arm 1 togoal: 28.8276295915455

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.612251e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.241925e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.777749823694723;-9.99290829930971;-9.99990881874731]
Robot Arm 1 end: [-9.93824214638256;-10.6999475254125;0.303731195960587]
Robot Arm 1 togoal: 28.7663531019124
Mobile Platform 2 Pose: [-2.27314250682276;9.95184935178593;9.98643597833539]
Robot Arm 2 end: [10.2595259943284;10.6289408151289;0.947085695453106]
Robot Arm 1 togoal: 28.9193874467964

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.415276e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.328417e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.085475145611;-10.5322721153291;0.143923539609885]
Robot Arm 1 togoal: 28.7558141881018
Mobile Platform 2 Pose: [-2.28545793734129;9.95755952981431;9.98974176533457]
Robot Arm 2 end: [10.0682769261184;9.757337608193;1.34832283732372]
Robot Arm 1 togoal: 28.1623497874379

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.211616e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.416979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.127191141507;-10.3865805745256;0.0460651194997101]
Robot Arm 1 togoal: 28.6860928617004
Mobile Platform 2 Pose: [-1.20192047910913;8.83051903106139;9.59345019430039]
Robot Arm 2 end: [8.76865963783706;9.05571829525037;0.0522666126650533]
Robot Arm 1 togoal: 26.7869401408968

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.495790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.021524e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.780664696541372;-9.99553613439038;-9.99860789106996]
Robot Arm 1 end: [-10.064640327137;-9.80455174935548;-0.108396972605055]
Robot Arm 1 togoal: 28.2394227268301
Mobile Platform 2 Pose: [-1.52346498357236;9.2308856476221;9.66530410673524]
Robot Arm 2 end: [9.45045575832835;9.7001354840956;1.2589755387759]
Robot Arm 1 togoal: 27.6854514884025

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.910323e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.853256e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785398161185857;-9.99999999852253;-10.0000000005638]
Robot Arm 1 end: [-10.163908783115;-9.65430278895186;-0.0318281308755881]
Robot Arm 1 togoal: 28.2008175544189
Mobile Platform 2 Pose: [-1.6725965138172;9.43267819857149;9.7935468760819]
Robot Arm 2 end: [9.40628896314193;10.0724529634994;1.17298183578132]
Robot Arm 1 togoal: 27.9218290220302

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.329616e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.566990e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.778625130053775;-9.99788053116563;-9.9894196248133]
Robot Arm 1 end: [-10.2194176846416;-9.29645345050394;0.620819037612809]
Robot Arm 1 togoal: 27.9640535873916
Mobile Platform 2 Pose: [-1.84474641573858;9.59411296198644;9.86987901788848]
Robot Arm 2 end: [9.46650744689135;10.232812967063;1.02874346049821]
Robot Arm 1 togoal: 28.0812027741105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.996832e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.726469e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.785398163434074;-9.9999999996456;-10.0000000005295]
Robot Arm 1 end: [-10.0120130517624;-9.27338278661192;0.730550523959556]
Robot Arm 1 togoal: 27.7951211689633
Mobile Platform 2 Pose: [-2.11737238943364;9.80640007133543;9.90443178761713]
Robot Arm 2 end: [9.35073165849295;10.2629072431617;0.939875292896423]
Robot Arm 1 togoal: 28.0245237531199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.887943e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.953480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.80894546940472;-9.30633152851892;0.83181211544239]
Robot Arm 1 togoal: 27.6695453095846
Mobile Platform 2 Pose: [-2.1303356288677;9.86075996163109;9.97587812689183]
Robot Arm 2 end: [9.20675358754337;9.51199309032917;0.887676441771755]
Robot Arm 1 togoal: 27.3864630147646

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.146814e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.393019e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.65261428067706;-9.39120793738375;0.900433106788723]
Robot Arm 1 togoal: 27.6157436366446
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10.5197260792667;10.4362042126247;1.16956110542677]
Robot Arm 1 togoal: 28.962408571886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.533457e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.908072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.785398164215636;-10.0000000003673;-10.0000000006376]
Robot Arm 1 end: [-9.7943206007917;-9.23502232735064;0.689793755595338]
Robot Arm 1 togoal: 27.6132597822376
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.90570245239607;10.1075219949467;0.565750123819421]
Robot Arm 1 togoal: 28.3101030239477

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.260643e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.684289e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.95461300094356;-9.17772909320372;0.742975395635223]
Robot Arm 1 togoal: 27.6871305027216
Mobile Platform 2 Pose: [-2.356194490624;9.99999999929421;9.99999999969185]
Robot Arm 2 end: [9.93292861366625;10.0410566303587;0.562459175283897]
Robot Arm 1 togoal: 28.2822275376499

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.024419e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.104326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785398164418788;-10.000000000548;-10.0000000001646]
Robot Arm 1 end: [-9.81222110479417;-9.24078420625901;0.855157929215215]
Robot Arm 1 togoal: 27.625626285033
Mobile Platform 2 Pose: [-1.66989544497555;9.62773745807208;9.79411028806356]
Robot Arm 2 end: [9.61275586557291;9.9330654802718;0.725689730224347]
Robot Arm 1 togoal: 27.9753429024307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.176860e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.117876e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785398162361147;-9.99999999916479;-10.0000000000717]
Robot Arm 1 end: [-9.7006687512018;-9.41857628057731;1.06249206565641]
Robot Arm 1 togoal: 27.6660023049055
Mobile Platform 2 Pose: [-2.19321999962763;9.89574919211769;9.84535568534255]
Robot Arm 2 end: [10.5785866851945;9.96883766791852;0.841353656647017]
Robot Arm 1 togoal: 28.6826537635515

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.552308e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.251856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.785398163905557;-10.0000000007452;-10.0000000001568]
Robot Arm 1 end: [-9.59672919641012;-9.44843457069319;0.944365349035418]
Robot Arm 1 togoal: 27.6153202039857
Mobile Platform 2 Pose: [-2.27275875949494;9.94779964357775;9.95469275449313]
Robot Arm 2 end: [10.2104145623326;10.2955091704806;1.22796186645247]
Robot Arm 1 togoal: 28.6435755557528

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.390273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.982752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.44768289764677;-9.44976257237737;0.727974181437857]
Robot Arm 1 togoal: 27.5160775576422
Mobile Platform 2 Pose: [-2.35619448540145;9.99999999576148;9.99999999875267]
Robot Arm 2 end: [10.2361545149572;10.2432862167555;1.27714809559345]
Robot Arm 1 togoal: 28.6243261957026

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.684439e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.789969e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [1.28658717823008;-10.2241745168661;-9.84964329555021]
Robot Arm 1 end: [-9.87608244453094;-9.12578416923133;0.905699081401086]
Robot Arm 1 togoal: 27.5904488664146
Mobile Platform 2 Pose: [-2.35619449019225;9.99999999999993;9.99999999999998]
Robot Arm 2 end: [10.3385669660024;10.2824920074943;1.22560887619703]
Robot Arm 1 togoal: 28.7249724835492

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.260482e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.909819e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.46947771849113;-9.3168031051697;0.66296645126371]
Robot Arm 1 togoal: 27.4396732197283
Mobile Platform 2 Pose: [-2.46217408649416;10.0500561759313;9.96365156881418]
Robot Arm 2 end: [9.75707060829518;10.1913007952528;1.43470189060997]
Robot Arm 1 togoal: 28.2495648495552

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.541419e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.172094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785398163421219;-10.0000000000323;-10.0000000000171]
Robot Arm 1 end: [-9.37319298411451;-9.42460134515412;0.439144737373206]
Robot Arm 1 togoal: 27.455530492547
Mobile Platform 2 Pose: [-2.31407795821826;9.9717411730916;9.98204169634509]
Robot Arm 2 end: [10.7142508570873;9.69686693594228;0.990016671818495]
Robot Arm 1 togoal: 28.5889611381275

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.127940e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.785398160902909;-9.99999999873324;-10.0000000002326]
Robot Arm 1 end: [-9.29811420307144;-9.49914105428424;0.517628609347482]
Robot Arm 1 togoal: 27.4525129596043
Mobile Platform 2 Pose: [-2.45003196603762;10.0239991345535;9.97514209273969]
Robot Arm 2 end: [10.0710217798827;10.4606032814546;0.0330968331028296]
Robot Arm 1 togoal: 28.7001055357582

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.476281e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.179273e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.785398163110208;-9.99999999975563;-10.0000000008705]
Robot Arm 1 end: [-9.24880965721595;-9.60292204637645;0.60026256462125]
Robot Arm 1 togoal: 27.4888883624084
Mobile Platform 2 Pose: [-2.28155700418947;9.94643864196742;9.99276228580856]
Robot Arm 2 end: [10.4367001112076;10.1306264389942;0.0827994873763756]
Robot Arm 1 togoal: 28.7222779907213

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.854203e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.937063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.361392370185286;-9.75216628585515;-10.0084431408281]
Robot Arm 1 end: [-9.605703161642;-9.87676878125246;1.25402532443966]
Robot Arm 1 togoal: 27.9202580418445
Mobile Platform 2 Pose: [-2.36479518266663;10.0011173564924;9.99436321674227]
Robot Arm 2 end: [10.6152521985542;9.78927134113044;0.209573979724325]
Robot Arm 1 togoal: 28.606358063017

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008598e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.324894e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.76239086873444;-9.92192884596676;1.28390117493338]
Robot Arm 1 togoal: 28.0622799794537
Mobile Platform 2 Pose: [-2.35333154716359;9.99333421395708;9.99689218803066]
Robot Arm 2 end: [10.3019722193696;10.0979594982217;0.105047053041908]
Robot Arm 1 togoal: 28.6024994491079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.319918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.325604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785398164527902;-10.0000000007637;-10.0000000004947]
Robot Arm 1 end: [-9.81645421149547;-9.75150388029963;1.26535210763338]
Robot Arm 1 togoal: 27.9799771067572
Mobile Platform 2 Pose: [-2.0559985279007;9.75984323016668;9.90994121131252]
Robot Arm 2 end: [10.1452672973912;9.99230667091618;0.795310654949326]
Robot Arm 1 togoal: 28.391032836669

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.053435e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.362054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.71351637350019;-9.65513480569118;1.16049578530281]
Robot Arm 1 togoal: 27.8402050158932
Mobile Platform 2 Pose: [-2.34113811588254;9.96902276697215;9.96062434708286]
Robot Arm 2 end: [10.0578182009075;9.88347137982365;0.825580954729934]
Robot Arm 1 togoal: 28.2515860216988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.497899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.339611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.623148433251767;-9.90138893577785;-9.99965049029244]
Robot Arm 1 end: [-9.35127878246427;-9.54554204532216;0.868914167512446]
Robot Arm 1 togoal: 27.5122776282706
Mobile Platform 2 Pose: [-2.35619449011172;10.000000000476;10.0000000004193]
Robot Arm 2 end: [10.129149181764;9.94516890994339;0.764583714717511]
Robot Arm 1 togoal: 28.3472145937572

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.852436e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.096106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.26591507946009;-9.58441353285011;-9.94793794765503]
Robot Arm 1 end: [-8.92929061937994;-10.3053720570829;0.749737271838633]
Robot Arm 1 togoal: 27.7708664597516
Mobile Platform 2 Pose: [-2.35619448911065;9.999999999188;9.99999999948448]
Robot Arm 2 end: [10.1260233884725;9.9731362720735;0.710968679163385]
Robot Arm 1 togoal: 28.3661618961201

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.375915e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.094935e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.733514359681545;-9.95849295702728;-9.98443485888523]
Robot Arm 1 end: [-9.2580485092436;-10.1210070689656;0.877433102113734]
Robot Arm 1 togoal: 27.8593168653827
Mobile Platform 2 Pose: [-2.35619448929673;9.99999999924317;10.0000000019303]
Robot Arm 2 end: [10.0849444857097;9.99288590302343;0.678044260100877]
Robot Arm 1 togoal: 28.351914505105

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.305691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.098443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.785398164870127;-10.0000000011781;-10.0000000001739]
Robot Arm 1 end: [-9.26819258167999;-9.92772602973971;0.778027331627806]
Robot Arm 1 togoal: 27.729578404062
Mobile Platform 2 Pose: [-2.27870137180863;9.93800550550731;10.0232479353008]
Robot Arm 2 end: [10.0835043887832;10.0410272561134;0.673961369248872]
Robot Arm 1 togoal: 28.384985405255

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.534795e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.557298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.652352081527816;-9.89356313450429;-10.0002594495175]
Robot Arm 1 end: [-10.44597467793;-10.2740163500851;0.159371727677597]
Robot Arm 1 togoal: 28.825815704782
Mobile Platform 2 Pose: [-1.65382159301352;9.43810168225694;10.0125606969589]
Robot Arm 2 end: [9.87806856215482;10.3434244924647;0.494687020470024]
Robot Arm 1 togoal: 28.4613044004363

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.828271e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.943597e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.712810178634118;-9.93613162655766;-10.0013148714176]
Robot Arm 1 end: [-10.0356248683219;-10.4217070490435;0.125020667792096]
Robot Arm 1 togoal: 28.6429946227175
Mobile Platform 2 Pose: [-2.24307724378303;9.90950620300412;10.0125606970247]
Robot Arm 2 end: [10.2577181970861;10.2784533619465;0.412000563932014]
Robot Arm 1 togoal: 28.6848513514432

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.054917e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.494786e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785398160606642;-9.99999999837569;-10.000000000657]
Robot Arm 1 end: [-10.1247937324233;-10.3613498224928;0.11865619530791]
Robot Arm 1 togoal: 28.6628410579459
Mobile Platform 2 Pose: [-2.35619448889907;9.99999999916072;10.0000000000741]
Robot Arm 2 end: [10.2831566772881;10.1802778935542;0.379317145978714]
Robot Arm 1 togoal: 28.6348301967317

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.144501e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.306063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.612113260373245;-9.84201356939584;-9.95699451758322]
Robot Arm 1 end: [-9.85180911310009;-9.9489338087795;0.697510918534188]
Robot Arm 1 togoal: 28.1555042476125
Mobile Platform 2 Pose: [-2.3561944907954;10.0000000005062;10.0000000003863]
Robot Arm 2 end: [10.2385660417204;10.1358800538874;0.287364214316514]
Robot Arm 1 togoal: 28.5757778235578

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.399001e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.730053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.785398162925852;-9.99999999962272;-9.99999999985852]
Robot Arm 1 end: [-9.99621904199585;-9.99978800571668;0.630149770969878]
Robot Arm 1 togoal: 28.2954750866574
Mobile Platform 2 Pose: [-2.35619449377997;10.0000000023679;10.0000000001484]
Robot Arm 2 end: [10.2143085286974;10.055062694172;0.219215783031815]
Robot Arm 1 togoal: 28.5047092794873

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.078135e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.349324e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.785398167265902;-10.0000000030046;-9.99999999885907]
Robot Arm 1 end: [-9.9348054944783;-10.0114729422013;0.590673223309977]
Robot Arm 1 togoal: 28.2616529475996
Mobile Platform 2 Pose: [-2.35619449282902;10.0000000018137;10.0000000008856]
Robot Arm 2 end: [10.2177869055652;9.96167646960797;0.174974958066075]
Robot Arm 1 togoal: 28.4436147184154

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.381856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785398165221712;-10.0000000015681;-10.000000000763]
Robot Arm 1 end: [-9.89313333879597;-10.042194604479;0.531947030163815]
Robot Arm 1 togoal: 28.2560532342375
Mobile Platform 2 Pose: [-2.16049058812558;9.85866993716476;9.98699613949076]
Robot Arm 2 end: [9.82399099171305;10.0404110063443;0.256632189405036]
Robot Arm 1 togoal: 28.2171458178037

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.228397e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.577638e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.785398164299224;-10.0000000002158;-10.0000000006868]
Robot Arm 1 end: [-9.89591342142668;-10.0926030987099;0.453705317092215]
Robot Arm 1 togoal: 28.2966285641127
Mobile Platform 2 Pose: [-2.35619449084226;10.0000000001509;10.000000000195]
Robot Arm 2 end: [9.8545510780282;10.0822691683227;0.351732908945723]
Robot Arm 1 togoal: 28.264251608511

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.980921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.406074103191991;-9.72283251571784;-9.98651655991672]
Robot Arm 1 end: [-10.0269436669292;-9.34366702275996;1.0300437086451]
Robot Arm 1 togoal: 27.8477461315236
Mobile Platform 2 Pose: [-2.32796826508611;9.98098183082007;10.0019234394713]
Robot Arm 2 end: [9.73241181924676;10.1383083544547;0.62202816978783]
Robot Arm 1 togoal: 28.2086456593535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.870748e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.920560e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.4087664984157;-9.31278920251907;0.80673630698024]
Robot Arm 1 togoal: 28.1071476566186
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.77513171880253;10.0320493775642;0.687192503435003]
Robot Arm 1 togoal: 28.1608606349751

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.013974e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.085936e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.731880721349895;-9.97211241161396;-9.99130968615085]
Robot Arm 1 end: [-9.9320181408284;-9.29616770589579;0.141836015249186]
Robot Arm 1 togoal: 27.7764203704725
Mobile Platform 2 Pose: [-2.35619449071819;10.0000000012186;9.99999999918002]
Robot Arm 2 end: [9.70728747311604;9.94238699880184;0.684694756399804]
Robot Arm 1 togoal: 28.0495166663971

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.039886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.444943e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.648591284334956;-9.92115121843241;-9.97277549913597]
Robot Arm 1 end: [-10.1747726332573;-10.3727691616637;1.23760137964603]
Robot Arm 1 togoal: 28.6731841429598
Mobile Platform 2 Pose: [-2.35619449555805;10.0000000030198;10.0000000005952]
Robot Arm 2 end: [9.65154227965182;9.96441158361942;0.709963409185385]
Robot Arm 1 togoal: 28.0253211257381

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.161916e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.561619e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.1344048065505;-10.2699174231984;1.34066368680637]
Robot Arm 1 togoal: 28.5709000635818
Mobile Platform 2 Pose: [-2.35619448828559;9.99999999848374;9.99999999988198]
Robot Arm 2 end: [9.64982298186082;9.83546381138218;0.7289356826723]
Robot Arm 1 togoal: 27.9318551785216

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.003127e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.288278e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.702955447776429;-9.93758171039817;-9.98187288368711]
Robot Arm 1 end: [-9.70640141281174;-9.24719734712591;0.363287819519297]
Robot Arm 1 togoal: 27.5705850473818
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.62712556773575;9.81840893801678;0.820804016461763]
Robot Arm 1 togoal: 27.9013201354612

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.986770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.530000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.785398163679491;-9.99999999999867;-10.0000000001117]
Robot Arm 1 end: [-9.61457156680949;-9.45987189804061;0.202273287834144]
Robot Arm 1 togoal: 27.6614727044498
Mobile Platform 2 Pose: [-2.23894223079154;9.91574440245384;9.99238276450212]
Robot Arm 2 end: [9.52407258690571;9.76343998815277;1.37456054595559]
Robot Arm 1 togoal: 27.7813681278979

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.816443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.151716868276675;-9.61675535841572;-9.97025644494776]
Robot Arm 1 end: [-9.95722121315219;-9.76586242333137;1.20203984137264]
Robot Arm 1 togoal: 28.0905986314202
Mobile Platform 2 Pose: [-1.19593895558771;9.26026173100249;9.84264796052163]
Robot Arm 2 end: [8.84490073414263;10.2793294328376;0.843575232996701]
Robot Arm 1 togoal: 27.6918831111247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.826795e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.800080e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.55287222942738;-9.87465297240376;-9.95735958840705]
Robot Arm 1 end: [-9.49925050919875;-9.72599603352679;0.953396372116233]
Robot Arm 1 togoal: 27.7427082999045
Mobile Platform 2 Pose: [-1.50468744799386;9.45943120119636;9.87317691634474]
Robot Arm 2 end: [9.4092068469117;10.216846867296;0.541917327842327]
Robot Arm 1 togoal: 28.0427675267763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.522896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.734407e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.858453284624126;-10.0367498877351;-9.95885076061876]
Robot Arm 1 end: [-10.8035025924133;-10.0849142167697;0.460958782180309]
Robot Arm 1 togoal: 28.9363644328555
Mobile Platform 2 Pose: [-2.06760805983441;9.79790551299559;9.92765918665394]
Robot Arm 2 end: [9.73287904892135;10.0950074696019;-0.0151425452932277]
Robot Arm 1 togoal: 28.2055947407988

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002471e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.897141e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785398163125083;-9.99999999978211;-9.99999999991829]
Robot Arm 1 end: [-10.7969836140547;-9.95727214060343;0.436874371374983]
Robot Arm 1 togoal: 28.8441080138736
Mobile Platform 2 Pose: [-2.35619448740837;9.99999999812002;9.99999999906229]
Robot Arm 2 end: [10.0112509562491;10.1805342587826;0.0576839925329637]
Robot Arm 1 togoal: 28.4577831452414

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.974097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.074033e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.785398163592271;-10.0000000004415;-10.00000000026]
Robot Arm 1 end: [-10.7945344811913;-9.92329820984112;0.355251722179464]
Robot Arm 1 togoal: 28.8220305477417
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.86054004810518;9.62506314036784;0.0151156574452315]
Robot Arm 1 togoal: 27.9616137216536

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000238e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.097272e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.757962960200684;-9.98632067735002;-9.99685954907041]
Robot Arm 1 end: [-10.8252089923369;-10.020111858229;0.504571171975254]
Robot Arm 1 togoal: 28.9054897199987
Mobile Platform 2 Pose: [-2.35619449091687;10.0000000002851;9.99999999986353]
Robot Arm 2 end: [9.95282881413936;9.49321707561119;0.103107299033235]
Robot Arm 1 togoal: 27.9304727735623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785398163655374;-10.0000000003085;-10.0000000000774]
Robot Arm 1 end: [-10.8357404263127;-10.0471621521746;0.414212168941142]
Robot Arm 1 togoal: 28.9351303569078
Mobile Platform 2 Pose: [-2.35264037527684;9.93108737082783;9.96666193706359]
Robot Arm 2 end: [9.60937454069361;10.2816360707065;0.35137197134283]
Robot Arm 1 togoal: 28.2354451232349

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.076504e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.785398163397449;-10;-10]
Robot Arm 1 end: [-10.6304331924831;-9.81612715385004;1.22103992647773]
Robot Arm 1 togoal: 28.6074054246933
Mobile Platform 2 Pose: [-2.35619449019231;9.99999999999998;10]
Robot Arm 2 end: [9.74934194880851;10.168467112594;0.290958147989533]
Robot Arm 1 togoal: 28.2544965632101

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.512475e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.785398163397434;-9.99999999999999;-10]
Robot Arm 1 end: [-10.5765912613273;-9.89921186328096;1.27726188849839]
Robot Arm 1 togoal: 28.62576023414
Mobile Platform 2 Pose: [-2.23778738725983;9.92719706130049;9.99336379263521]
Robot Arm 2 end: [9.46993456829355;9.55998982277275;0.877982520231821]
Robot Arm 1 togoal: 27.6058875140774

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.647762e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.329189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.663485074243528;-9.90698839495209;-10.014846708947]
Robot Arm 1 end: [-10.5989120220008;-10.3197568281506;0.325704696952945]
Robot Arm 1 togoal: 28.9592200327769
Mobile Platform 2 Pose: [-2.3425068022888;9.97012143690731;10.0012167649651]
Robot Arm 2 end: [9.60970018922981;9.86581732874405;1.00698307730311]
Robot Arm 1 togoal: 27.9187258492813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.422023e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.757488377474214;-9.97332827902577;-10.0064809029518]
Robot Arm 1 end: [-10.3905739419233;-10.5680424299769;0.380471863531043]
Robot Arm 1 togoal: 28.9848353369904
Mobile Platform 2 Pose: [-2.2583654756885;9.90615893790275;10.0140991576334]
Robot Arm 2 end: [9.79804773571708;10.3067298397272;-0.0880629616419438]
Robot Arm 1 togoal: 28.4062502725004

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.104738e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.095453e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.770015570354844;-9.98820937138986;-10.0004211021812]
Robot Arm 1 end: [-10.2530517916278;-9.93589069367604;-0.184198652765441]
Robot Arm 1 togoal: 28.469870865003
Mobile Platform 2 Pose: [-2.06683712169214;9.81210372175403;10.0045501373169]
Robot Arm 2 end: [9.92225211539722;9.69202131466322;0.566916602971879]
Robot Arm 1 togoal: 28.0282376887147

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.808525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.450734e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [0.742796839251859;-9.97454001020851;-9.99347899888741]
Robot Arm 1 end: [-10.3718242782475;-10.0608947662865;-0.049221270443411]
Robot Arm 1 togoal: 28.6341809396737
Mobile Platform 2 Pose: [-2.35619448534892;9.99999999750891;10.0000000013665]
Robot Arm 2 end: [10.0048636675814;9.72816326966001;0.609764189950113]
Robot Arm 1 togoal: 28.1109471034755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.966476e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.235105e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.5046560138667;-10.0058296771698;0.0151139372192365]
Robot Arm 1 togoal: 28.686962755806
Mobile Platform 2 Pose: [-2.35619449452262;10.0000000034798;10.0000000013172]
Robot Arm 2 end: [9.87279910985225;9.70707757152354;0.591626293974511]
Robot Arm 1 togoal: 28.0028710377354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.947888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.286335e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [0.763593545580399;-9.99008810090689;-9.99512828526358]
Robot Arm 1 end: [-10.5296679855459;-9.81214668936836;0.0980207761297334]
Robot Arm 1 togoal: 28.5659464031802
Mobile Platform 2 Pose: [-2.35619449026608;9.99999999991727;9.99999999959809]
Robot Arm 2 end: [9.73490959205453;9.76072066051282;0.611640478583244]
Robot Arm 1 togoal: 27.9424349756526

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985966e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.244577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [0.774446901057435;-9.99632937701173;-9.99586192072301]
Robot Arm 1 end: [-10.4862560211902;-9.73689290019534;0.0955197062453509]
Robot Arm 1 togoal: 28.4826898516874
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.76994209743726;10.2160225977219;0.171040020148186]
Robot Arm 1 togoal: 28.308312774781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000102e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.471161e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [0.737728897232451;-9.96445072775566;-10.0098370208196]
Robot Arm 1 end: [-10.1045926265905;-10.1566980398801;-0.143708580140408]
Robot Arm 1 togoal: 28.517685305944
Mobile Platform 2 Pose: [-2.35619449008736;9.9999999999852;9.99999999981386]
Robot Arm 2 end: [9.82633549770177;10.2914764528396;0.132147098525503]
Robot Arm 1 togoal: 28.4034594422142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.004326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.805400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [0.74583252810709;-9.96676189479242;-10.011785978948]
Robot Arm 1 end: [-9.68089669192618;-10.1587512996635;-0.15389910872083]
Robot Arm 1 togoal: 28.2226546516154
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.82645590072221;10.3563190319219;0.0838536591265114]
Robot Arm 1 togoal: 28.452301624258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.693400e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.599783e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [0.785398162233904;-9.99999999902085;-10.0000000000332]
Robot Arm 1 end: [-9.57100014438695;-10.0404929282221;-0.0937444663847539]
Robot Arm 1 togoal: 28.0580256459615
Mobile Platform 2 Pose: [-2.35619449019235;10;10]
Robot Arm 2 end: [9.81474681001927;10.3822389126233;0.0360913268723235]
Robot Arm 1 togoal: 28.4651507637664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.777278e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.909206e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [0.838977869854601;-10.0271967049794;-9.98917468588751]
Robot Arm 1 end: [-9.25740251926;-10.2211925201485;0.290332755923441]
Robot Arm 1 togoal: 27.9510057169926
Mobile Platform 2 Pose: [-2.35619449019234;9.99999999999999;10]
Robot Arm 2 end: [9.84634995469178;10.347507681744;-0.000623953098967511]
Robot Arm 1 togoal: 28.4642585506345

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.848980e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.012302e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [0.785232186323759;-9.9985898871806;-9.99817419074789]
Robot Arm 1 end: [-9.14023483437363;-10.127022272918;0.525946829598208]
Robot Arm 1 togoal: 27.7927282917931
Mobile Platform 2 Pose: [-2.30646899049763;9.9693872354272;10.0021445823228]
Robot Arm 2 end: [9.50697485731147;10.2848511055957;0.141663780446417]
Robot Arm 1 togoal: 28.17622793893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.527089e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.603506e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [0.785398163397449;-10;-10]
Robot Arm 1 end: [-9.13011856375811;-9.97232528699052;0.611068999468606]
Robot Arm 1 togoal: 27.6709809739108
Mobile Platform 2 Pose: [-2.35619449019234;9.99999999999999;10]
Robot Arm 2 end: [9.45848693101842;10.3194265089257;0.259367709223574]
Robot Arm 1 togoal: 28.1620937313506

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.536917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.136943e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [0.785398164394094;-10.0000000008433;-10.0000000001965]
Robot Arm 1 end: [-9.18135133116762;-9.8304817048711;0.822564317085564]
Robot Arm 1 togoal: 27.5981893598127
Mobile Platform 2 Pose: [-2.24815557173111;9.92646626371093;10.0105150446859]
Robot Arm 2 end: [9.20863203206334;9.92421812236048;0.333807200356766]
Robot Arm 1 togoal: 27.7011811855403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.487559e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.935796e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [0.725506474680715;-9.95208664902661;-9.98203249338498]
Robot Arm 1 end: [-9.30940299976083;-9.70789720107087;0.47420415362709]
Robot Arm 1 togoal: 27.6106870222226
Mobile Platform 2 Pose: [-2.35619448623376;9.99999999679594;9.99999999817886]
Robot Arm 2 end: [9.26223232750318;10.045174755362;0.453939675257105]
Robot Arm 1 togoal: 27.8205183802953

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.734240e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.028115e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [0.785398163397449;-10;-10]
Robot Arm 1 end: [-9.46048692120964;-9.67888262536672;0.287872124932701]
Robot Arm 1 togoal: 27.7036022377921
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.25533786804842;9.92352141744553;0.576921575682783]
Robot Arm 1 togoal: 27.7237448075719

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.975948e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [0.725071865271823;-9.95310844720389;-9.98495506410631]
Robot Arm 1 end: [-9.88226045389427;-10.5168850044131;1.10830081694177]
Robot Arm 1 togoal: 28.5730147456739
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.27650715657072;9.8123308275363;0.732236938210187]
Robot Arm 1 togoal: 27.6538302617325
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('optim.options.SolverOptions/setProperty')" style="font-weight:bold">optim.options.SolverOptions/setProperty</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('optim.options.MultiAlgorithm/set.Algorithm', '/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/+optim/+options/MultiAlgorithm.m', 56)" style="font-weight:bold">optim.options.MultiAlgorithm/set.Algorithm</a> (<a href="matlab: opentoline('/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/+optim/+options/MultiAlgorithm.m',56,0)">第 56 行</a>)
             obj = setProperty(obj, 'Algorithm', value);

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('optim.options.SolverOptions', '/home/zzh/MATLAB/R2022a/toolbox/matlab/optimfun/+optim/+options/SolverOptions.m', 364)" style="font-weight:bold">optim.options.SolverOptions</a> (<a href="matlab: opentoline('/home/zzh/MATLAB/R2022a/toolbox/matlab/optimfun/+optim/+options/SolverOptions.m',364,0)">第 364 行</a>)
                        obj.(pvPairs{i}) = pvPairs{i+1};

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('optim.options.MultiAlgorithm', '/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/+optim/+options/MultiAlgorithm.m', 42)" style="font-weight:bold">optim.options.MultiAlgorithm</a> (<a href="matlab: opentoline('/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/+optim/+options/MultiAlgorithm.m',42,0)">第 42 行</a>)
            obj = obj@optim.options.SolverOptions(varargin{:});

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('optim.options.Fmincon', '/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/+optim/+options/Fmincon.m', 387)" style="font-weight:bold">optim.options.Fmincon</a> (<a href="matlab: opentoline('/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/+optim/+options/Fmincon.m',387,0)">第 387 行</a>)
            obj = obj@optim.options.MultiAlgorithm(varargin{:});

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('optim.options.createSolverOptions', '/home/zzh/MATLAB/R2022a/toolbox/matlab/optimfun/+optim/+options/createSolverOptions.m', 114)" style="font-weight:bold">optim.options.createSolverOptions</a> (<a href="matlab: opentoline('/home/zzh/MATLAB/R2022a/toolbox/matlab/optimfun/+optim/+options/createSolverOptions.m',114,0)">第 114 行</a>)
options = optionsCreationFcn(varargin{:});

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('optimoptions', '/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/optimoptions.m', 102)" style="font-weight:bold">optimoptions</a> (<a href="matlab: opentoline('/home/zzh/MATLAB/R2022a/toolbox/shared/optimlib/optimoptions.m',102,0)">第 102 行</a>)
    options = optim.options.createSolverOptions(solverName, varargin{:});

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('mobile_robot_CBLF', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m', 28)" style="font-weight:bold">mobile_robot_CBLF</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/Safety_control/mobile_robot_CBLF.m',28,0)">第 28 行</a>)
    options = optimoptions('fmincon', 'Algorithm', 'sqp', 'Display', 'notify');

位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 102)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',102,0)">第 102 行</a>)
    [dx1] = mobile_robot_CBLF(mobile_robot1, a1_G, obstacle_results1, D_min, V_max, dt);
} 
main_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.430805e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.259141566892;-10.0035524791063;1.47381471675359]
Robot Arm 1 togoal: 28.4706367203596
Mobile Platform 2 Pose: [-2.35513367867696;9.99878051588958;8.23801711864745]
Robot Arm 2 end: [10.2577275179429;8.22736714556422;1.47381471675359]
Robot Arm 1 togoal: 27.2509571153266

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.826232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.785398163816203;-10.0000000004024;-9.99999999998701]
Robot Arm 1 end: [-10.4383911944831;-9.89000075687976;1.3825988437274]
Robot Arm 1 togoal: 28.5194516007489
Mobile Platform 2 Pose: [-1.87631651139127;9.6717102110394;8.72857308564147]
Robot Arm 2 end: [9.76948427890357;8.62108573688647;-0.0969720186586785]
Robot Arm 1 togoal: 27.2065281895257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.013430e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.480438470987;-9.87299582418333;1.36188030313921]
Robot Arm 1 togoal: 28.5378633059629
Mobile Platform 2 Pose: [-1.94803131803224;9.71314515728478;9.08433048699783]
Robot Arm 2 end: [10.5129463279926;8.96451488985515;0.887992557384634]
Robot Arm 1 togoal: 27.9434158700229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.747736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.785398163542596;-10.0000000000439;-9.99999999998629]
Robot Arm 1 end: [-10.5056015524634;-9.79946133438362;1.31710470099772]
Robot Arm 1 togoal: 28.5050861681401
Mobile Platform 2 Pose: [-1.95159924009444;9.69437136150077;9.37053120633663]
Robot Arm 2 end: [9.51650141211831;9.37101401585156;-0.00570929001795301]
Robot Arm 1 togoal: 27.5401679847411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.744145e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.4258244916062;-9.7068942851601;1.30974479787038]
Robot Arm 1 togoal: 28.3834567536938
Mobile Platform 2 Pose: [-1.95159923373312;9.6943713596576;9.72408459098951]
Robot Arm 2 end: [9.69899680217554;9.69414046450331;-0.0789205972130403]
Robot Arm 1 togoal: 27.9010643088233

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.915324e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.2905006738355;-9.81246969326846;1.4170519057284]
Robot Arm 1 togoal: 28.3592873349791
Mobile Platform 2 Pose: [-2.23363474522032;9.90195220402238;9.96323207650839]
Robot Arm 2 end: [10.1163411418827;9.85565662734733;-0.0864703641736347]
Robot Arm 1 togoal: 28.3107442857199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [0.78539816440861;-10.0000000005907;-9.99999999998682]
Robot Arm 1 end: [-10.2757265175814;-9.93179103862621;1.46568108554116]
Robot Arm 1 togoal: 28.432102279035
Mobile Platform 2 Pose: [-2.33973723342554;9.97617793821157;9.97222602384192]
Robot Arm 2 end: [10.3337649855818;9.89946910142855;1.35684285980335]
Robot Arm 1 togoal: 28.4513236348374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.465129e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.1370202992013;-9.97274463497696;1.50568202970272]
Robot Arm 1 togoal: 28.3621287886622
Mobile Platform 2 Pose: [-2.35619448847573;9.99999999871485;10.0000000004086]
Robot Arm 2 end: [10.1513126525353;10.0265668223723;1.43723346934713]
Robot Arm 1 togoal: 28.4103114580398

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.591475e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.0551559445985;-10.0012133461152;1.519169546625]
Robot Arm 1 togoal: 28.324156097394
Mobile Platform 2 Pose: [-2.356194491933;10.0000000016281;9.99999999987039]
Robot Arm 2 end: [10.2925853257351;9.83604371022225;1.37250562601594]
Robot Arm 1 togoal: 28.3774505524226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.751158e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [0.785398160406159;-9.9999999980484;-10.000000000492]
Robot Arm 1 end: [-10.1577853159938;-10.0360315719418;1.50027934455254]
Robot Arm 1 togoal: 28.4214584309656
Mobile Platform 2 Pose: [-2.16662849394522;9.84711538060929;9.98939848405017]
Robot Arm 2 end: [9.70967057995131;9.79494564783442;-0.0259141928814985]
Robot Arm 1 togoal: 27.9768463392688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.140356e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [0.785398164306627;-10.0000000008884;-10.000000000539]
Robot Arm 1 end: [-10.1903456929746;-10.1074784169038;1.47938599527051]
Robot Arm 1 togoal: 28.4949553292062
Mobile Platform 2 Pose: [-2.35619449191381;10.0000000013772;10.0000000005164]
Robot Arm 2 end: [9.83216450169924;9.75132188322237;0.0620946225681336]
Robot Arm 1 togoal: 28.0278052986212

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.035696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.2626524681405;-10.0816342379948;1.4379545688129]
Robot Arm 1 togoal: 28.5281267393582
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.43674853007165;9.61427137684802;0.358999411898335]
Robot Arm 1 togoal: 27.637964477013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.191495e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [0.78539816322453;-9.99999999986166;-9.99999999994813]
Robot Arm 1 end: [-10.16799961524;-10.1397942345808;1.47939727254454]
Robot Arm 1 togoal: 28.5019517064798
Mobile Platform 2 Pose: [-1.52100915866252;9.50862843007278;9.84549279481367]
Robot Arm 2 end: [9.90876215055106;9.65984413894495;0.156964879852519]
Robot Arm 1 togoal: 28.013012578868

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.726877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.2360643918858;-10.1388485920794;1.43825843595122]
Robot Arm 1 togoal: 28.5495774198791
Mobile Platform 2 Pose: [-1.99789042471195;9.7590559836857;9.9288485905984]
Robot Arm 2 end: [10.3113301280832;9.8125816200287;0.755822967983125]
Robot Arm 1 togoal: 28.3843974378989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.744161e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.1333955561188;-10.1731007678077;1.47940058902255]
Robot Arm 1 togoal: 28.5010410637623
Mobile Platform 2 Pose: [-2.21426504483568;9.88387394133248;9.96492873234559]
Robot Arm 2 end: [10.3215223873473;10.5423792160491;0.594621520481378]
Robot Arm 1 togoal: 28.9104097702322

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.472533e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [0.785398162891652;-9.99999999959536;-10.0000000001517]
Robot Arm 1 end: [-10.1701152267314;-10.2289705826261;1.43425527220641]
Robot Arm 1 togoal: 28.5666295481314
Mobile Platform 2 Pose: [-2.2194235283764;9.90910070648075;9.98627333442458]
Robot Arm 2 end: [9.84261622308394;10.375742353622;0.0971164652663154]
Robot Arm 1 togoal: 28.4767929963727

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.940680e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.91744549114871;-10.1437431568604;1.42892543741756]
Robot Arm 1 togoal: 28.3281397375033
Mobile Platform 2 Pose: [-1.94163361374442;9.7581260900839;9.8869085829469]
Robot Arm 2 end: [10.0404561315395;10.2398375871999;0.503200880058298]
Robot Arm 1 togoal: 28.5009968696701

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [0.785398163865563;-10.0000000004417;-9.99999999982335]
Robot Arm 1 end: [-9.99184044898549;-10.3197357554521;1.3870737104134]
Robot Arm 1 togoal: 28.5058464522654
Mobile Platform 2 Pose: [-1.28543709210972;9.34389922759024;9.92916144242914]
Robot Arm 2 end: [9.67209771861916;10.3114698559851;1.08713155110313]
Robot Arm 1 togoal: 28.2795947317574

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.878828e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [0.785398164545579;-10.0000000009392;-10.0000000000623]
Robot Arm 1 end: [-9.92622305721069;-10.2849503605123;1.43612003003549]
Robot Arm 1 togoal: 28.4348515217411
Mobile Platform 2 Pose: [-1.86647077192029;9.68190000962281;9.95540017749173]
Robot Arm 2 end: [9.58637064096025;10.6579256001601;0.490680730147198]
Robot Arm 1 togoal: 28.4857396390659

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.647053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [0.785398162450288;-9.99999999936124;-10.0000000000729]
Robot Arm 1 end: [-9.78282132409714;-10.1460635713861;1.44243374312899]
Robot Arm 1 togoal: 28.2352628749973
Mobile Platform 2 Pose: [-1.8647416936366;9.71126358911955;9.95238575960693]
Robot Arm 2 end: [10.3699696263976;9.79566504953477;0.388044857801063]
Robot Arm 1 togoal: 28.4268817264217

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.250297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [0.7853981626984;-9.99999999923836;-9.99999999987156]
Robot Arm 1 end: [-9.7987294389733;-10.0368176917526;1.46464314092273]
Robot Arm 1 togoal: 28.1685449873749
Mobile Platform 2 Pose: [-2.23643119524461;9.92967126452487;9.98331222725414]
Robot Arm 2 end: [10.5366000901584;9.93551090013846;1.05709960274856]
Robot Arm 1 togoal: 28.6250195055166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.164455e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.70215592649089;-10.1559563081784;1.41418834497951]
Robot Arm 1 togoal: 28.1859704743087
Mobile Platform 2 Pose: [-2.35619448777823;9.99999999801334;9.9999999992204]
Robot Arm 2 end: [10.4853339730985;10.0491050257566;1.20669393594063]
Robot Arm 1 togoal: 28.6655596251862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.100039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [0.78539816301582;-9.99999999971595;-10.0000000004201]
Robot Arm 1 end: [-9.77410355979988;-10.020418167922;1.45792465670346]
Robot Arm 1 togoal: 28.139585883788
Mobile Platform 2 Pose: [-2.34391295255961;9.9785437388291;9.98424238522839]
Robot Arm 2 end: [9.22639381736935;9.93796843546333;0.666240301847023]
Robot Arm 1 togoal: 27.7111425007288

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.288111e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [0.785398164166827;-10.0000000005336;-10.0000000002061]
Robot Arm 1 end: [-9.77908088189064;-10.1252384373505;1.47461855862442]
Robot Arm 1 togoal: 28.2177145412361
Mobile Platform 2 Pose: [-2.19438457634526;9.87161501852322;10.0021391245442]
Robot Arm 2 end: [10.0990271299884;10.3878008442101;1.39741768983139]
Robot Arm 1 togoal: 28.6295055392609

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.280765e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [0.785398156830157;-9.99999999419811;-10.0000000023007]
Robot Arm 1 end: [-9.91017319379221;-10.0098203649742;1.50422591560185]
Robot Arm 1 togoal: 28.2277910757332
Mobile Platform 2 Pose: [-2.35619448582463;9.99999999633823;10.0000000013097]
Robot Arm 2 end: [10.2760641365849;10.5332776399239;1.27910781519128]
Robot Arm 1 togoal: 28.8581492708416

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.329847e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [0.785398163369359;-9.99999999972753;-10.0000000000896]
Robot Arm 1 end: [-9.75924159049472;-10.0322104884664;1.47814448532812]
Robot Arm 1 togoal: 28.1375002777591
Mobile Platform 2 Pose: [-2.00517257295172;9.79126051190465;9.98777319153487]
Robot Arm 2 end: [9.7298286884416;10.2520599432547;-0.0680775578478621]
Robot Arm 1 togoal: 28.3184964202778

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.959746e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.71332279235046;-10.218828231991;1.43073995155444]
Robot Arm 1 togoal: 28.2387014125385
Mobile Platform 2 Pose: [-2.35619449255416;10.0000000009573;9.99999999991515]
Robot Arm 2 end: [9.94357640914677;10.1168776863405;-0.105075934219914]
Robot Arm 1 togoal: 28.3739163806922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.425929e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [0.785398164864108;-10.0000000011733;-10.000000001131]
Robot Arm 1 end: [-9.72625685202973;-10.0630108298745;1.46618559001454]
Robot Arm 1 togoal: 28.1363220334758
Mobile Platform 2 Pose: [-2.35619449032089;9.99999999980699;10.0000000003881]
Robot Arm 2 end: [9.93830179016463;10.2915993099369;-0.0437437698743779]
Robot Arm 1 togoal: 28.4909337140094

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.670598e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [0.785398161029181;-9.99999999817909;-10.0000000004199]
Robot Arm 1 end: [-9.71332278084681;-10.2188282386222;1.430739947029]
Robot Arm 1 togoal: 28.2387014092702
Mobile Platform 2 Pose: [-2.35619449085596;10.0000000006712;10.0000000002432]
Robot Arm 2 end: [9.8909954324561;10.4384422196747;0.0621867284492979]
Robot Arm 1 togoal: 28.5571314354615

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.219655e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.93137655865621;-10.1568293264023;1.4779052332981]
Robot Arm 1 togoal: 28.3471232610221
Mobile Platform 2 Pose: [-2.35619449347541;10.000000002418;9.99999999947488]
Robot Arm 2 end: [9.85443275439591;10.3489575235107;0.0321935885839035]
Robot Arm 1 togoal: 28.4691959286374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.792221e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [0.785398164185405;-10.0000000002399;-9.99999999978146]
Robot Arm 1 end: [-9.94754100617866;-10.3175644780202;1.40997390442933]
Robot Arm 1 togoal: 28.4731477941162
Mobile Platform 2 Pose: [-2.33132111207003;9.98656315136693;9.99126486228542]
Robot Arm 2 end: [9.74683431781542;10.1047838952356;0.199821942925529]
Robot Arm 1 togoal: 28.2114393909257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.837121e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.77403039190568;-10.4119874860751;1.35455849409212]
Robot Arm 1 togoal: 28.4198735719669
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.68321364833933;10.0480762009952;0.177451427662115]
Robot Arm 1 togoal: 28.1275555577322

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.763450e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.85924570522429;-10.5759035043233;1.22581514667201]
Robot Arm 1 togoal: 28.5979820808624
Mobile Platform 2 Pose: [-2.35619449213784;9.99999999935663;10.0000000012632]
Robot Arm 2 end: [9.64045625398416;9.97077530892262;0.154113440031008]
Robot Arm 1 togoal: 28.0436760638109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.486230e-03','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.766045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [0.785398163477385;-10.0000000000733;-9.99999999991565]
Robot Arm 1 end: [-9.9772128421247;-10.6967479905902;1.04681756407631]
Robot Arm 1 togoal: 28.7692415490588
Mobile Platform 2 Pose: [-2.35619448474858;9.99999999596288;9.99999999668426]
Robot Arm 2 end: [9.71139625677692;9.83458195001859;0.0893439079895765]
Robot Arm 1 togoal: 27.9999896908815

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.104158e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.034048e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [0.785398163499721;-10.0000000000205;-9.99999999996932]
Robot Arm 1 end: [-10.1240286743525;-10.7374516329415;0.837784760449322]
Robot Arm 1 togoal: 28.9047610849546
Mobile Platform 2 Pose: [-2.35619452787615;10.0000000275822;10.0000000028791]
Robot Arm 2 end: [9.87029281071446;9.69128847834284;0.0325737267677532]
Robot Arm 1 togoal: 28.014117710392

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.002145e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.613485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [0.785398163184196;-9.99999999974818;-9.99999999985481]
Robot Arm 1 end: [-10.2701034535735;-10.6761441979932;0.620075467256637]
Robot Arm 1 togoal: 28.9688056006539
Mobile Platform 2 Pose: [-1.7981680598082;9.62196611750725;9.71618510795729]
Robot Arm 2 end: [9.8528702608392;9.64943274480678;-0.0956369991579152]
Robot Arm 1 togoal: 27.9794599657221

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.966430e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [0.785398165166293;-10.0000000010072;-10.0000000000825]
Robot Arm 1 end: [-10.339356606741;-10.5420943347894;0.417061078302785]
Robot Arm 1 togoal: 28.928977651801
Mobile Platform 2 Pose: [-2.06213682954141;9.76158552251906;9.82295395185902]
Robot Arm 2 end: [10.090174550922;9.36664787339638;0.0568397686186758]
Robot Arm 1 togoal: 27.9432626678024

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.108000e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.304840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [0.785398163397449;-10;-10]
Robot Arm 1 end: [-10.314773814837;-10.3884831777269;0.264171982256262]
Robot Arm 1 togoal: 28.8090245947798
Mobile Platform 2 Pose: [-2.29329377456012;9.93157889774098;9.93082385222552]
Robot Arm 2 end: [10.1863959723422;9.73291507879554;-0.105214840196433]
Robot Arm 1 togoal: 28.2758394139878

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.740471e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.289463e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [0.785398163397449;-10;-10]
Robot Arm 1 end: [-9.55892978245641;-10.1725851366594;0.244994303908655]
Robot Arm 1 togoal: 28.126733117575
Mobile Platform 2 Pose: [-2.37872339676903;10.0018612698158;9.96706389391922]
Robot Arm 2 end: [10.1655555878032;9.25915649498052;0.579543606337027]
Robot Arm 1 togoal: 27.9007362125823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.672737e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.388358e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [0.785398163534415;-10.0000000000735;-10.0000000001332]
Robot Arm 1 end: [-9.61548642469622;-9.98252539949899;0.081590164978324]
Robot Arm 1 togoal: 28.0381976897985
Mobile Platform 2 Pose: [-2.43179661685105;10.0551464326054;9.93867120968524]
Robot Arm 2 end: [9.3696439709756;10.0646866685454;0.740043557356659]
Robot Arm 1 togoal: 27.8995457245196

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.280480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.181555e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [0.784843356181794;-9.99955615422748;-9.99983355783531]
Robot Arm 1 end: [-9.91709509809327;-10.3115217066495;-0.040269097600278]
Robot Arm 1 togoal: 28.4901062192001
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.3024481839925;10.0239976123262;0.544009292283334]
Robot Arm 1 togoal: 27.829831065192

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.899805e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.434549e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [0.785398163256336;-9.99999999982585;-9.9999999998964]
Robot Arm 1 end: [-9.87933610804294;-10.377737284944;0.0178965974988648]
Robot Arm 1 togoal: 28.5078848880331
Mobile Platform 2 Pose: [-2.14251659134493;9.90032787080286;9.96736044320823]
Robot Arm 2 end: [10.0836760735438;9.33921426759278;0.0791514588676901]
Robot Arm 1 togoal: 27.9184200901567

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.590887e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.005410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [0.785398163269839;-9.99999999997448;-10.0000000000383]
Robot Arm 1 end: [-9.75377538760103;-10.3210335327619;-0.0260888988400526]
Robot Arm 1 togoal: 28.382204459209
Mobile Platform 2 Pose: [-2.31865214028677;9.95268204317116;9.99659069914215]
Robot Arm 2 end: [9.40163189204711;10.0088197871843;1.08890444535153]
Robot Arm 1 togoal: 27.8740541708389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.584552e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.704523e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.57208284516259;-10.3185127178222;0.114024049030915]
Robot Arm 1 togoal: 28.2469107565556
Mobile Platform 2 Pose: [-2.2372178338448;9.93093123672771;9.98392180067106]
Robot Arm 2 end: [10.3877514043262;9.44054664484334;0.812958389816329]
Robot Arm 1 togoal: 28.1797193757618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.576866e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.046176e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [0.796226459239769;-10.0006132026298;-9.99570769027669]
Robot Arm 1 end: [-9.1738324095268;-10.1451176769894;0.382345024314409]
Robot Arm 1 togoal: 27.8344777325177
Mobile Platform 2 Pose: [-2.35619449092545;9.99999999993773;9.99999999962225]
Robot Arm 2 end: [10.3352111473685;9.36666129060561;0.658661011833879]
Robot Arm 1 togoal: 28.095053131888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.767994e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.683558e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [0.7853981638652;-10.0000000003742;-9.9999999997241]
Robot Arm 1 end: [-9.14744982225253;-10.0683470306255;0.48792530942108]
Robot Arm 1 togoal: 27.7566321904083
Mobile Platform 2 Pose: [-2.35619448986921;9.99999999984329;10.0000000002169]
Robot Arm 2 end: [10.4139028728039;9.44189660414173;0.502097477476737]
Robot Arm 1 togoal: 28.2090931512662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.104354e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.885090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [0.785398164525227;-10.0000000012743;-9.99999999964618]
Robot Arm 1 end: [-9.1603978752348;-10.0213932711528;0.556739992114617]
Robot Arm 1 togoal: 27.7291693459843
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10.5147595829333;9.54073334341939;0.427600559582381]
Robot Arm 1 togoal: 28.3529741417101

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.262472e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.149339e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.185610089582;-9.88511262716435;0.784572667004821]
Robot Arm 1 togoal: 27.6414121187986
Mobile Platform 2 Pose: [-2.32642604940353;9.97701788658506;10.0040787041025]
Robot Arm 2 end: [10.3802395737082;9.43080995248625;0.500529080325023]
Robot Arm 1 togoal: 28.177152116374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.199374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.170520e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [0.785398163270931;-9.99999999975664;-9.99999999941352]
Robot Arm 1 end: [-9.24364109780083;-10.0957044659598;0.939194320561548]
Robot Arm 1 togoal: 27.8297243704064
Mobile Platform 2 Pose: [-2.36823580023578;10.0050378636305;9.99660305886543]
Robot Arm 2 end: [10.1243092023915;9.57341180332381;0.0974174007902666]
Robot Arm 1 togoal: 28.1093019819548

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.404567e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [0.785398150706026;-9.99999998923205;-9.9999999999649]
Robot Arm 1 end: [-9.19903726821932;-10.19603720638;0.771280371799631]
Robot Arm 1 togoal: 27.8755274459313
Mobile Platform 2 Pose: [-2.38429784262184;10.0156900514747;9.9840129319084]
Robot Arm 2 end: [9.88411879716696;10.6858373308423;0.708452844560447]
Robot Arm 1 togoal: 28.7043947732351

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.280111e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.025016e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [0.785398159738505;-9.99999999768142;-9.99999999966287]
Robot Arm 1 end: [-9.24467542486706;-10.0899219178528;0.943556141336628]
Robot Arm 1 togoal: 27.8261735832258
Mobile Platform 2 Pose: [-2.39437890830712;10.0228824435344;9.98731977381288]
Robot Arm 2 end: [9.93214024798293;10.1578897064701;1.28727702690987]
Robot Arm 1 togoal: 28.3493449432544

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.349069e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [0.192188219252916;-9.52561290293592;-10.0011862866941]
Robot Arm 1 end: [-8.81739348935683;-10.323649741802;0.770244983500219]
Robot Arm 1 togoal: 27.7075561833868
Mobile Platform 2 Pose: [-2.33009932085361;9.98690568345609;9.96970637719526]
Robot Arm 2 end: [9.89846018813024;9.66529378742796;0.0627757830379811]
Robot Arm 1 togoal: 28.0142627151134

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.792423e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.164702e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [0.504909962361416;-9.77560943917117;-9.98310361339652]
Robot Arm 1 end: [-10.1918202480559;-10.6676235157013;0.642481152812147]
Robot Arm 1 togoal: 28.9073011898626
Mobile Platform 2 Pose: [-2.30704172667062;9.95883828570472;9.98389236914697]
Robot Arm 2 end: [9.70906366649946;9.66574327595771;1.1993274139687]
Robot Arm 1 togoal: 27.8440679940739

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006700e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.656879e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [0.680795572072207;-9.92503488471562;-9.99167572360712]
Robot Arm 1 end: [-10.5441564461744;-10.3457760495285;0.410342388514794]
Robot Arm 1 togoal: 28.9352125873028
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.95308856606143;9.47719561761257;1.09630284313746]
Robot Arm 1 togoal: 27.8866860728715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.040236e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.827056e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [0.785398159849679;-9.99999999739634;-9.99999999988862]
Robot Arm 1 end: [-10.6589866174889;-10.3042571136177;0.510960437765648]
Robot Arm 1 togoal: 28.9840777827199
Mobile Platform 2 Pose: [-2.34750234463898;9.96722487896424;9.94196383129401]
Robot Arm 2 end: [9.75906523312499;10.7691942455292;0.632560829301055]
Robot Arm 1 togoal: 28.6804709280626

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.971486e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002218e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [0.785398163072068;-9.99999999973969;-9.99999999990727]
Robot Arm 1 end: [-10.6877554859151;-10.205362876982;0.705603056535496]
Robot Arm 1 togoal: 28.9293067357472
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10.0440784398307;10.8514121187966;0.501919032744683]
Robot Arm 1 togoal: 28.9410606900958

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.984724e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [0.711152765014704;-9.94779700420037;-9.99696394613132]
Robot Arm 1 end: [-10.2312768896773;-10.1058874047874;1.17760505720396]
Robot Arm 1 togoal: 28.5248872567012
Mobile Platform 2 Pose: [-2.35619448808545;9.9999999984174;10.0000000009752]
Robot Arm 2 end: [9.95668281580298;10.8334770601394;0.373727072070813]
Robot Arm 1 togoal: 28.8724642279989

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.807756e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [0.78539816503648;-10.0000000014037;-10.0000000003102]
Robot Arm 1 end: [-10.1173336525487;-10.1135809677059;1.22615324513912]
Robot Arm 1 togoal: 28.449080604499
Mobile Platform 2 Pose: [-2.35619448909073;9.99999999943733;9.99999999984348]
Robot Arm 2 end: [10.0575671549288;10.7499413116853;0.196064311863076]
Robot Arm 1 togoal: 28.8898168770687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [0.635558473883296;-9.89225604163674;-9.99370314266509]
Robot Arm 1 end: [-9.26132933220161;-10.0424879236142;0.174286642775353]
Robot Arm 1 togoal: 27.8300874356923
Mobile Platform 2 Pose: [-2.3561944900187;9.99999999986109;9.99999999996298]
Robot Arm 2 end: [9.9426727558035;10.7579884258171;0.256236262436449]
Robot Arm 1 togoal: 28.8132592297935

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.656097e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [0.769953965981535;-9.98251214957327;-9.99395529713688]
Robot Arm 1 end: [-9.46386538710632;-10.1045866475239;0.0538081294343114]
Robot Arm 1 togoal: 28.0212260984462
Mobile Platform 2 Pose: [-2.3561944893072;9.99999999975302;9.99999999911375]
Robot Arm 2 end: [9.83425929952182;10.7351914244518;0.327872084861315]
Robot Arm 1 togoal: 28.7188014999096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.220712e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005267e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.35152429549946;-10.2574062085749;0.20534057497082]
Robot Arm 1 togoal: 28.0459437099411
Mobile Platform 2 Pose: [-2.25039105190854;9.93923278346315;10.0006873573188]
Robot Arm 2 end: [9.2719919361297;10.4000727883951;0.773247613370808]
Robot Arm 1 togoal: 28.0736847704354

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.462543e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.114082e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [0.78539816352259;-10.0000000001349;-9.99999999982871]
Robot Arm 1 end: [-9.37295950240873;-10.4388438045638;0.329799795760999]
Robot Arm 1 togoal: 28.1864657925733
Mobile Platform 2 Pose: [-2.35619449640764;10.0000000047444;9.9999999999037]
Robot Arm 2 end: [9.25414528927843;10.2339566314756;0.714029585576123]
Robot Arm 1 togoal: 27.942553803023

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.120617e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.958476e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [0.730996537371545;-9.94934747606096;-10.0058981166679]
Robot Arm 1 end: [-9.57644244157718;-10.1678500736635;1.0424939072422]
Robot Arm 1 togoal: 28.1106428838431
Mobile Platform 2 Pose: [-1.97771047969476;9.69466515148896;10.0630075635164]
Robot Arm 2 end: [10.2058442240986;9.71171818473743;0.622285809525167]
Robot Arm 1 togoal: 28.2424443311695

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.865958e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.931752e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [0.785398164288012;-10.0000000005039;-9.99999999980166]
Robot Arm 1 end: [-9.70873343589119;-10.1114233960858;1.01698628757403]
Robot Arm 1 togoal: 28.1630530036421
Mobile Platform 2 Pose: [-2.35619448715348;9.99999999783199;10.0000000028748]
Robot Arm 2 end: [10.6258695947874;9.78035568472147;0.606886006971699]
Robot Arm 1 togoal: 28.5923866081081

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.173561e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.878197e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [0.893151337599303;-10.0611555465984;-9.97618897306891]
Robot Arm 1 end: [-10.0926403886297;-9.88843750790702;0.72891261570514]
Robot Arm 1 togoal: 28.2823539811854
Mobile Platform 2 Pose: [-2.33056528610196;9.95615362172126;9.98865405059444]
Robot Arm 2 end: [9.41433639607893;10.4361493181434;0.562259058818714]
Robot Arm 1 togoal: 28.2041103537386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.853068e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.638438e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [0.782305676651222;-9.99752601060302;-9.99907225397613]
Robot Arm 1 end: [-10.4777846397189;-9.91671944391124;0.148833651466267]
Robot Arm 1 togoal: 28.5989198685235
Mobile Platform 2 Pose: [-2.35619448870924;9.99999999987984;10.0000000007712]
Robot Arm 2 end: [9.5791319785375;10.5446122161179;0.386398001975727]
Robot Arm 1 togoal: 28.4026551899214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.735641e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.121976e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [0.78539816376088;-10.0000000002337;-10.0000000001206]
Robot Arm 1 end: [-10.413568049201;-9.83339864541255;0.112963637866357]
Robot Arm 1 togoal: 28.4966669757062
Mobile Platform 2 Pose: [-2.35619449705556;10.0000000045703;9.9999999987683]
Robot Arm 2 end: [9.73770307479998;10.7190954132379;0.539406387022839]
Robot Arm 1 togoal: 28.6325228268985

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.951617e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.390480e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [0.7853981636308;-10.0000000001867;-9.99999999996842]
Robot Arm 1 end: [-10.4022414562346;-9.70410709785773;0.108295502491876]
Robot Arm 1 togoal: 28.3989265106909
Mobile Platform 2 Pose: [-2.35619448965385;10.0000000000552;9.99999999998149]
Robot Arm 2 end: [9.90337210750872;10.7408457592337;0.337175745650335]
Robot Arm 1 togoal: 28.770268402006

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.701829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.275878e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [0.785398163523186;-9.99999999999856;-10.0000000000377]
Robot Arm 1 end: [-10.4008156093168;-9.52822270361381;0.232119857605568]
Robot Arm 1 togoal: 28.270231189517
Mobile Platform 2 Pose: [-2.1600037241112;9.89146815119106;9.96255918911961]
Robot Arm 2 end: [9.42942113341547;9.52694522754745;0.964370343598072]
Robot Arm 1 togoal: 27.552020471712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.645262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.881823e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [0.785398163253343;-9.99999999988471;-9.99999999995677]
Robot Arm 1 end: [-10.3446516532407;-9.36402192776646;0.416338884927459]
Robot Arm 1 togoal: 28.1085480288449
Mobile Platform 2 Pose: [-2.23520863977383;9.93250461583887;9.97662196021802]
Robot Arm 2 end: [10.0667447502028;10.0731921204061;1.35679621376369]
Robot Arm 1 togoal: 28.3836968990289

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.989175e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.816499e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [0.769025499154135;-9.99082061855389;-9.99898534168458]
Robot Arm 1 end: [-10.3178282463399;-9.28539355036576;0.642931479684211]
Robot Arm 1 togoal: 28.0269790267632
Mobile Platform 2 Pose: [-2.35619449019234;9.99999999999999;10]
Robot Arm 2 end: [9.98810159888484;9.99032188471506;1.39503638344704]
Robot Arm 1 togoal: 28.2692950286093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.990520e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.930297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [0.757502884740259;-9.98349911740201;-10.0018326136294]
Robot Arm 1 end: [-10.2792713026928;-9.23379633813269;0.61379668813005]
Robot Arm 1 togoal: 27.9644557249744
Mobile Platform 2 Pose: [-2.35619449019234;9.99999999999999;10]
Robot Arm 2 end: [9.8683420344214;9.88243609931405;1.39176200677864]
Robot Arm 1 togoal: 28.108343652712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.557293e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.099374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10.1234493446923;-9.21460712463487;0.383072386648056]
Robot Arm 1 togoal: 27.8468888699563
Mobile Platform 2 Pose: [-2.03162874235294;9.76059525153254;9.99900168617965]
Robot Arm 2 end: [9.78901518831589;10.0625101840735;0.678384447092424]
Robot Arm 1 togoal: 28.1925422404482

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.925272e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [0.793282948996826;-10.004544173013;-9.99942428331035]
Robot Arm 1 end: [-10.0546655734398;-9.22187541011462;0.359667474008012]
Robot Arm 1 togoal: 27.8032156194281
Mobile Platform 2 Pose: [-1.65253909376224;9.52574924024822;9.94313797619318]
Robot Arm 2 end: [9.32461572619011;9.53949803367946;0.225904212892089]
Robot Arm 1 togoal: 27.5119979185602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.707996e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [0.810455254268621;-10.0143176945374;-9.99947579252488]
Robot Arm 1 end: [-9.97892030876804;-9.16356097023553;0.536383433365391]
Robot Arm 1 togoal: 27.701422265785
Mobile Platform 2 Pose: [-2.17185216262172;9.84466244725099;10.0089944043531]
Robot Arm 2 end: [10.4838888165781;9.87718472564447;0.224730061594493]
Robot Arm 1 togoal: 28.5722328392539

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.683378e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.839024e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [0.785398163951017;-10.000000000326;-9.9999999999148]
Robot Arm 1 end: [-9.80593442293302;-9.166690313553;0.752576935984639]
Robot Arm 1 togoal: 27.5722238846721
Mobile Platform 2 Pose: [-2.26188646553521;9.92881401463766;10.0050527973769]
Robot Arm 2 end: [10.5463787938639;9.53693561100231;0.405625175632267]
Robot Arm 1 togoal: 28.374100789421

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.715845e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.995375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-9.95417238421972;-9.18819601392126;0.955863879730281]
Robot Arm 1 togoal: 27.6889010370989
Mobile Platform 2 Pose: [-2.35619448922734;9.9999999998514;10.000000000102]
Robot Arm 2 end: [10.7352613342694;9.64923485136688;0.600343663894174]
Robot Arm 1 togoal: 28.5813068444014

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.271708e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.434579e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [0.785398162900722;-9.99999999951722;-9.99999999934623]
Robot Arm 1 end: [-9.71580213524555;-9.32489418386427;1.08126029772068]
Robot Arm 1 togoal: 27.6108268630163
Mobile Platform 2 Pose: [-2.35619449006382;10.0000000000096;10.0000000000386]
Robot Arm 2 end: [10.7965263290959;9.90723492194184;0.689520484201511]
Robot Arm 1 togoal: 28.8007789439687

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.814252e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.813627e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [1.07458695562823;-10.1492609329797;-9.92992490024297]
Robot Arm 1 end: [-9.65792080282563;-9.99004025985911;0.874525025206094]
Robot Arm 1 togoal: 28.0437780937945
Mobile Platform 2 Pose: [-2.21578803605418;9.91169769346531;10.0058423103045]
Robot Arm 2 end: [9.98652440078904;10.3331570337975;0.141673655988777]
Robot Arm 1 togoal: 28.5447188416297

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.750754e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [0.974195483952735;-10.0906567995325;-9.95572194617274]
Robot Arm 1 end: [-9.89986846077773;-9.82695041255971;0.608985674987681]
Robot Arm 1 togoal: 28.105951283445
Mobile Platform 2 Pose: [-2.4497034846271;10.0537845408799;9.97171783640066]
Robot Arm 2 end: [9.91057008166349;10.4486519947012;0.32263209375181]
Robot Arm 1 togoal: 28.5659632257684

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.573412e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [0.733451913790142;-9.96469564279609;-10.0107543449545]
Robot Arm 1 end: [-9.99582656500838;-10.0657853122348;0.60767730849692]
Robot Arm 1 togoal: 28.3426000669206
Mobile Platform 2 Pose: [-2.30488962305348;9.96777263457123;9.99937323446091]
Robot Arm 2 end: [9.96229662430755;10.2859886688174;0.808015441236824]
Robot Arm 1 togoal: 28.4696851013365

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.728217e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.449705e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [0.785398165300949;-10.0000000023578;-10.0000000003671]
Robot Arm 1 end: [-9.97895928126812;-10.0417696019817;0.577217749622803]
Robot Arm 1 togoal: 28.3146970260608
Mobile Platform 2 Pose: [-2.35619448794328;9.99999999919394;9.99999999942202]
Robot Arm 2 end: [9.88568884909827;10.2339128775671;0.795135529497651]
Robot Arm 1 togoal: 28.3791953827036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.571145e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.276132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [0.785398168109252;-10.000000003018;-10.0000000015535]
Robot Arm 1 end: [-9.97165310424293;-10.0695022785248;0.545576764955588]
Robot Arm 1 togoal: 28.330254144843
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.55682290634702;10.4409498935563;0.776557378671804]
Robot Arm 1 togoal: 28.2993983956823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.621159e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985887e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [0.785398163732323;-9.99999999979881;-10.0000000005542]
Robot Arm 1 end: [-10.0094912501839;-10.1560952835055;0.560189878241793]
Robot Arm 1 togoal: 28.4177950105037
Mobile Platform 2 Pose: [-2.34142654005476;9.99202102325475;9.99830709589323]
Robot Arm 2 end: [9.45957155147832;10.1862453172677;0.302677693933108]
Robot Arm 1 togoal: 28.0649912563228

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.445150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.261104e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [0.785398163826084;-10.0000000002455;-9.99999999825365]
Robot Arm 1 end: [-10.081884746014;-10.2204507320385;0.550476162766345]
Robot Arm 1 togoal: 28.5147442435949
Mobile Platform 2 Pose: [-2.33784067639598;9.98931795546013;9.99880741119433]
Robot Arm 2 end: [9.51578186485255;10.119883583074;0.237997057650875]
Robot Arm 1 togoal: 28.0592507694527

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.021967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.670659e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [0.853288012049093;-10.049707128153;-10.0198560460581]
Robot Arm 1 end: [-10.7140932917465;-10.2463657768415;0.263243390555982]
Robot Arm 1 togoal: 28.992601465847
Mobile Platform 2 Pose: [-2.30549184443815;9.96634472251501;10.0010160296397]
Robot Arm 2 end: [10.1342780262676;9.96089242877759;-0.0283041977034427]
Robot Arm 1 togoal: 28.3941318178474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.164845e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.330844e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [0.785398164457968;-10.0000000006722;-9.99999999994731]
Robot Arm 1 end: [-10.5670461889503;-10.0109381768701;0.133311780190178]
Robot Arm 1 togoal: 28.7291961992035
Mobile Platform 2 Pose: [-2.35619449010622;9.9999999998201;9.9999999994652]
Robot Arm 2 end: [10.0047758101613;10.0422479346563;-0.0221732276399142]
Robot Arm 1 togoal: 28.3595511441834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.502918e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.640358e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [0.785398163397449;-10;-10]
Robot Arm 1 end: [-10.672606641056;-9.93969581521121;0.300289335453418]
Robot Arm 1 togoal: 28.7478393808217
Mobile Platform 2 Pose: [-2.35619449000448;9.99999999990027;9.99999999986503]
Robot Arm 2 end: [10.0521561185111;10.034226343525;0.0102977242586553]
Robot Arm 1 togoal: 28.3855845729952

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.685390e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.148962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [0.775436473996183;-9.98945751941689;-10.0033807410513]
Robot Arm 1 end: [-10.6035222811811;-10.1372406376933;0.301136444843284]
Robot Arm 1 togoal: 28.8357704593564
Mobile Platform 2 Pose: [-2.35619449076125;10.0000000006347;10.0000000002382]
Robot Arm 2 end: [10.0883573591304;10.0482937275436;0.0509706714583806]
Robot Arm 1 togoal: 28.4189579154614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.724385e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [0.785398166418543;-10.0000000027858;-10.0000000005313]
Robot Arm 1 end: [-10.6187625083938;-10.305027522537;0.482497690721634]
Robot Arm 1 togoal: 28.9569680235157
Mobile Platform 2 Pose: [-2.27495506553359;9.94305506527718;9.98032260850264]
Robot Arm 2 end: [9.99254110886557;9.9983414339048;0.577112883383272]
Robot Arm 1 togoal: 28.2935731745856

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.508991e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.109132e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [0.70116768442975;-10.022509179007;-9.96671828687206]
Robot Arm 1 end: [-9.31324099372481;-9.7765939104648;0.813773014011748]
Robot Arm 1 togoal: 27.6516747122274
Mobile Platform 2 Pose: [-2.3561944899055;9.99999999958791;9.99999999970081]
Robot Arm 2 end: [9.98570981598014;9.9937417374372;0.545428359625194]
Robot Arm 1 togoal: 28.286570056078

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.057584e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.911652e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [0.727437375302968;-9.98053294332662;-9.98469437935665]
Robot Arm 1 end: [-9.68731069916994;-9.93515807543694;1.20811411984113]
Robot Arm 1 togoal: 28.0196114825369
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [9.91632053620344;9.99396722031485;0.53089786266546]
Robot Arm 1 togoal: 28.2382515597226

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000220e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.367271e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [0.785398162079531;-9.99999999904219;-10.0000000004439]
Robot Arm 1 end: [-9.67432726423906;-9.9569262765188;1.22304152942689]
Robot Arm 1 togoal: 28.0258245009308
Mobile Platform 2 Pose: [-2.35619449033332;10.0000000003658;10.0000000002585]
Robot Arm 2 end: [9.87847663637231;9.9857825868999;0.479573012292042]
Robot Arm 1 togoal: 28.2076214730054

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.726612e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.074175e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [1.11115542953217;-10.2014034556131;-9.97373508710203]
Robot Arm 1 end: [-10.7396027148982;-9.51608784771005;0.407533679545445]
Robot Arm 1 togoal: 28.4999756648692
Mobile Platform 2 Pose: [-2.35619448978669;9.99999999944026;9.99999999996984]
Robot Arm 2 end: [9.83578398130395;9.86927095123798;0.457578401266426]
Robot Arm 1 togoal: 28.0958559183257
{操作在以下过程中被用户终止 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('real_time_2D_plot')" style="font-weight:bold">real_time_2D_plot</a>


位置 <a href="matlab:matlab.internal.language.introspective.errorDocCallback('main_matlab', '/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m', 90)" style="font-weight:bold">main_matlab</a> (<a href="matlab: opentoline('/home/zzh/文档/Adaptive security constraints developed based on the threats with the greatest relative velocity/zzh_tex_0423/code/optimizedHumanRobotInteraction/matlab/main_matlab.m',90,0)">第 90 行</a>)
    real_time_2D_plot(human_poses{k-1}, mobile_robot1, mobile_robot2, dis2goal, target1(2:3), target2(2:3))
} 
mwith

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','7.546079e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','6.136064e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','3.570603e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.374705e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','1.258519e-08','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','2.000415e-08','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','2.004550e-08','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.518205e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','2.009926e-08','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','2.003340e-08','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.040621e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.023364e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','7.253012e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.003168e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.098184e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','5.651359e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.064353e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.085879e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.540160e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','5.431348e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','6.376819e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','7.397215e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','7.674379e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.023379e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.269145e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.428700e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.861740e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.687993e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.764211e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.824070e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.894476e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.993721e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.044324e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.107853e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.197954e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.231414e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.597939e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.369802e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.379947e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.421952e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.845569e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.524421e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.878641e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.598597e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.638431e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.667025e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.832553e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.553686e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.990986e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.782569e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.814051e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.812763e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.862042e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.912096e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.748461e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.833188e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.930328e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.790530e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.886239e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.927071e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.932400e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.959173e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.950244e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.918717e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.928075e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.930470e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.952669e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.925996e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.904692e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.926822e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.939244e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.919990e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.904839e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.889857e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.861653e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.831950e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.817618e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.742530e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.865299e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.931154e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.888831e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.850294e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.724874e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.713696e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.697168e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.561639e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.641916e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.646568e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.472465e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.486853e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.798032e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.436698e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.446791e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.760491e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.533614e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.620383e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.344691e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.647692e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.299349e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.191986e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.125824e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.510651e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.768221e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.392744e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','3.368865e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.744106e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.525526e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.036917e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.255032e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','6.226172e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000000e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000000e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.991890e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_poss_with_constr','CSHelpWindow');">Local minimum possible. Constraints satisfied</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> and constraints are 
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit2basic','fmincon'},{'optimlib:sqpLineSearch:Exit2detailed','1.000000e-10','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000000e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','3.200017e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.291498e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.842734e-08','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','6.964983e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.571923e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000000e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.200970e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','1.407821e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000152e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.254028e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.070592e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.021825e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.011609e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000000e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000000e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000048e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000122e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000264e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.022195e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','8.253548e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','6.400000e-08','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','7.852369e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.000928e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.001064e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.154447e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','9.636516e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.143127e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.005330e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.004073e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.039212e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','solver_stopped_premature','CSHelpWindow');">Solver stopped prematurely</a>.

fmincon stopped because it exceeded the function evaluation limit,
<a href = "matlab: helpview('optim','max_fun_evals','CSHelpWindow');">options.MaxFunctionEvaluations</a> = 3.000000e+03.


<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.158467e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.007988e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.005030e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.004398e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.009386e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.003361e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.003858e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.005304e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.006281e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.006048e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.026949e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.009710e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.008903e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.010180e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.011439e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.013005e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.015020e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.006316e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.020284e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.022734e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.025936e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.029674e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.047472e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.038286e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.043613e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.047892e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.072990e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.064157e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.073502e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.113876e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.092806e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.106905e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.119868e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.136826e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.157694e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.180092e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.194822e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.222475e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.260000e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.367166e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.299824e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.332826e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.533181e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.466776e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.513268e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.526899e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.609247e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.683502e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','local_min_found','CSHelpWindow');">Local minimum found that satisfies the constraints</a>.

Optimization completed because the objective function is non-decreasing in 
<a href = "matlab: helpview('optim','feasible_directions','CSHelpWindow');">feasible directions</a>, to within the value of the <a href = "matlab: helpview('optim','optimality_tolerance_msgcsh','CSHelpWindow');">optimality tolerance</a>,
and constraints are satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:Exit1basic'},{'optimlib:sqpLineSearch:Exit1detailed','4.752176e-07','1.000000e-06','0.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
failmain_matlab

ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]


ans = 

  包含以下字段的 <a href="matlab:helpPopup struct" style="font-weight:bold">struct</a>:

           Tc2a: [4×4 double]
              w: {[3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]  [3×1 double]}
              M: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
     joint_base: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
    joint_world: {[4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]  [4×4 double]}
          Slist: [6×7 double]
              q: [7×1 double]
             dq: [7×1 double]
        J_space: [6×7 double]
         J_body: [6×7 double]

Step 2:
Mobile Platform 1 Pose: [0.785398163397448;-10;-10]
Robot Arm 1 end: [-10;-10;1.521]
Robot Arm 1 togoal: 28.2842712474619
Mobile Platform 2 Pose: [-2.35619449019234;10;10]
Robot Arm 2 end: [10;10;1.521]
Robot Arm 1 togoal: 28.2842712474619

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.881808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 3:
Mobile Platform 1 Pose: [0.785228159289654;-9.9998844326399;-9.75000002671158]
Robot Arm 1 end: [-10.2590260100714;-9.7535517369108;1.47381471675359]
Robot Arm 1 togoal: 28.295461960366
Mobile Platform 2 Pose: [-2.35481212876771;9.99829268820467;7.31185698828475]
Robot Arm 2 end: [10.2572397500226;7.30120846843936;1.47381471675359]
Robot Arm 1 togoal: 26.6400038139871

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.530000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 4:
Mobile Platform 1 Pose: [0.413543008197901;-9.74991229854279;-9.74626722839916]
Robot Arm 1 end: [-10.1875803705378;-9.63342511507218;1.3825988352036]
Robot Arm 1 togoal: 28.1607694825353
Mobile Platform 2 Pose: [-1.83133484038258;9.63948502364978;7.30719612570906]
Robot Arm 2 end: [10.4095450939037;7.04502323854301;0.466289491012015]
Robot Arm 1 togoal: 26.6119289492575

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.944241e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 5:
Mobile Platform 1 Pose: [0.380708377089659;-9.71840118780857;-9.76078650336839]
Robot Arm 1 end: [-10.1850026522751;-9.63147941850473;1.37049648064301]
Robot Arm 1 togoal: 28.1576271646044
Mobile Platform 2 Pose: [-1.83582395250143;9.63734710153411;7.30519787655005]
Robot Arm 2 end: [10.2380551436595;6.96507042386802;1.11544236776417]
Robot Arm 1 togoal: 26.4113037822382

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 6:
Mobile Platform 1 Pose: [0.139303032026906;-9.48784127196464;-9.85744117323001]
Robot Arm 1 end: [-9.9794158886142;-9.6541560383201;1.32654862316824]
Robot Arm 1 togoal: 28.0267857623167
Mobile Platform 2 Pose: [-2.13110263079181;9.75637617394424;7.25333322453245]
Robot Arm 2 end: [9.27560580677326;6.9631104073179;1.28798743000308]
Robot Arm 1 togoal: 25.6778190033099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.772924e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 7:
Mobile Platform 1 Pose: [0.0297905183443635;-9.42046644012155;-9.85547286151884]
Robot Arm 1 end: [-9.86811389028013;-9.53577380989645;1.28280621336205]
Robot Arm 1 togoal: 27.864765281419
Mobile Platform 2 Pose: [-2.04318361032218;9.54455827719666;7.30511972450293]
Robot Arm 2 end: [10.0024336873354;7.6126967498304;0.203887444815542]
Robot Arm 1 togoal: 26.684063140773

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.539668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 8:
Mobile Platform 1 Pose: [-0.179814241038298;-9.29089524215855;-9.85870051111235]
Robot Arm 1 end: [-9.61250463590919;-9.6486555903653;1.39882570484487]
Robot Arm 1 togoal: 27.762113232979
Mobile Platform 2 Pose: [-1.56174703104534;9.30313644355365;7.37008107963594]
Robot Arm 2 end: [9.72039129185023;7.82847157813661;0.373744753595656]
Robot Arm 1 togoal: 26.6094800046245

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.413866e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 9:
Mobile Platform 1 Pose: [-0.63822689431419;-9.04282291666583;-9.82771446578336]
Robot Arm 1 end: [-9.35422363470522;-9.70485713364185;1.44651286649513]
Robot Arm 1 togoal: 27.6201541540317
Mobile Platform 2 Pose: [-1.09433267133251;9.0598897053127;7.42848316564541]
Robot Arm 2 end: [9.42542110999212;7.98440878774455;0.467428483044677]
Robot Arm 1 togoal: 26.493319114195

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.814232e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 10:
Mobile Platform 1 Pose: [-1.08616429009036;-8.81562285162767;-9.72340660010783]
Robot Arm 1 end: [-9.08522001174274;-9.70088886899765;1.47843385132253]
Robot Arm 1 togoal: 27.4294086156124
Mobile Platform 2 Pose: [-0.828247914353247;8.92559702770344;7.49217752137825]
Robot Arm 2 end: [9.42806163419951;7.98463152982077;0.568575505523589]
Robot Arm 1 togoal: 26.4915771999962

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.318588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 11:
Mobile Platform 1 Pose: [-1.39172707047226;-8.56925083218319;-9.76584276437083]
Robot Arm 1 end: [-8.91124604109854;-9.6498018049456;1.44049284998833]
Robot Arm 1 togoal: 27.2718979757884
Mobile Platform 2 Pose: [-0.412245112608875;8.71474305424521;7.56197035905494]
Robot Arm 2 end: [9.25707872688288;7.98986108430525;0.755974086477541]
Robot Arm 1 togoal: 26.3639042551426

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.296763e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 12:
Mobile Platform 1 Pose: [-1.45038915351002;-8.51908881354904;-9.7872501730621]
Robot Arm 1 end: [-8.60704026376938;-9.72934910847437;1.50009230248159]
Robot Arm 1 togoal: 27.1195427828975
Mobile Platform 2 Pose: [-0.514787487186938;8.74702870101702;7.51210648727873]
Robot Arm 2 end: [8.96522820034021;8.17064975605587;0.243410248295508]
Robot Arm 1 togoal: 26.2960953151203

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.502360e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 13:
Mobile Platform 1 Pose: [-1.82931639338305;-8.26909753083998;-9.78933789210155]
Robot Arm 1 end: [-8.31007874638745;-9.75417678873935;1.51652193809308]
Robot Arm 1 togoal: 26.9348566796394
Mobile Platform 2 Pose: [-0.470801961078026;8.71286252167769;7.46419678580262]
Robot Arm 2 end: [8.73075834936944;6.69690789726796;0.625690212671712]
Robot Arm 1 togoal: 25.1083575983697

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.184329e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 14:
Mobile Platform 1 Pose: [-1.7891559828945;-8.31214196431818;-9.58391710673617]
Robot Arm 1 end: [-8.14821442050004;-9.54491548905011;1.49037394588771]
Robot Arm 1 togoal: 26.6713769063719
Mobile Platform 2 Pose: [-0.713436218330834;8.75217484000575;7.46609791402979]
Robot Arm 2 end: [8.77968548610553;7.64035652499235;-0.09173138119504]
Robot Arm 1 togoal: 25.8158801475715

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 15:
Mobile Platform 1 Pose: [-1.57418123437846;-8.40151851025372;-9.40435704310059]
Robot Arm 1 end: [-8.46227304919724;-9.34406028060045;1.51375277239934]
Robot Arm 1 togoal: 26.7403860631242
Mobile Platform 2 Pose: [-0.597885974495903;8.6753134370415;7.3694594896414]
Robot Arm 2 end: [8.43254247549479;7.17198536050029;1.14423559152692]
Robot Arm 1 togoal: 25.1947941994303

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.974492e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 16:
Mobile Platform 1 Pose: [-1.48069684908909;-8.4752305038266;-9.29773609734555]
Robot Arm 1 end: [-8.45028083818465;-9.2879975965117;1.51386963072557]
Robot Arm 1 togoal: 26.6915673037964
Mobile Platform 2 Pose: [0.246729108187685;8.14721366583422;7.216738048037]
Robot Arm 2 end: [7.81741733580993;7.36795880656253;1.16864295980488]
Robot Arm 1 togoal: 24.8843426497195

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.861151e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 17:
Mobile Platform 1 Pose: [-1.26367621968501;-8.59171342165568;-9.07653092142372]
Robot Arm 1 end: [-8.65293857881709;-9.01772963916598;1.51388551356366]
Robot Arm 1 togoal: 26.638434805173
Mobile Platform 2 Pose: [1.36873309956545;7.5692594769654;7.19978585485705]
Robot Arm 2 end: [7.70639328789051;7.5846584489649;1.17453076314836]
Robot Arm 1 togoal: 24.9570955234036

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.422986e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 18:
Mobile Platform 1 Pose: [-1.19357993634272;-8.65295487527022;-8.94386668190262]
Robot Arm 1 end: [-8.63664366570473;-8.9517312162618;1.51409241460471]
Robot Arm 1 togoal: 26.5799294756517
Mobile Platform 2 Pose: [1.39632271257143;7.55254107071155;7.19910485171916]
Robot Arm 2 end: [7.85479132542009;7.63154851376368;1.0881110174605]
Robot Arm 1 togoal: 25.096861738845

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.487155e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 19:
Mobile Platform 1 Pose: [-1.54457868647427;-8.41449471040006;-9.01894497284036]
Robot Arm 1 end: [-8.30588646860619;-9.12077534812166;1.50429046305802]
Robot Arm 1 togoal: 26.470923832074
Mobile Platform 2 Pose: [1.39713566377694;7.54845591462512;7.1886137107116]
Robot Arm 2 end: [7.81686486142953;7.74844821466868;0.979525375383264]
Robot Arm 1 togoal: 25.1543491724259

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.026739e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 20:
Mobile Platform 1 Pose: [-1.69381032044776;-8.30433312853864;-9.01150857327525]
Robot Arm 1 end: [-8.02599796132347;-9.20019119299927;1.44702779345488]
Robot Arm 1 togoal: 26.3360858184489
Mobile Platform 2 Pose: [1.84022738737876;7.29681083140285;7.21400825713954]
Robot Arm 2 end: [7.70292282043236;7.66843354446439;0.414861026675256]
Robot Arm 1 togoal: 25.0357856623204

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.639020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 21:
Mobile Platform 1 Pose: [-1.70382659233728;-8.29405803931669;-9.00875303672127]
Robot Arm 1 end: [-8.0057580354049;-9.09733431482338;1.46406367217636]
Robot Arm 1 togoal: 26.2472616115873
Mobile Platform 2 Pose: [2.54118572693354;6.91514315724906;7.20949963650416]
Robot Arm 2 end: [6.5592817947571;7.17950397282524;0.339985058326211]
Robot Arm 1 togoal: 23.8901646416041

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 22:
Mobile Platform 1 Pose: [-2.1199368610381;-8.05680147544976;-9.08755229372987]
Robot Arm 1 end: [-7.83800327060015;-9.16322892344561;1.47681493071835]
Robot Arm 1 togoal: 26.1806351293217
Mobile Platform 2 Pose: [2.77906130201984;6.89600636271097;7.00209254557173]
Robot Arm 2 end: [6.70419216325554;6.79746730176061;0.00518272935328895]
Robot Arm 1 togoal: 23.7377893995771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.555298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 23:
Mobile Platform 1 Pose: [-1.69549331125529;-8.25259161577457;-8.93209740975515]
Robot Arm 1 end: [-7.82170303745459;-9.0270415867332;1.37163362571873]
Robot Arm 1 togoal: 26.0703609682002
Mobile Platform 2 Pose: [3.31687204277896;6.44535450622308;6.98922297861097]
Robot Arm 2 end: [6.08230278861202;6.77839220161457;0.0690471254018614]
Robot Arm 1 togoal: 23.286542787753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 24:
Mobile Platform 1 Pose: [-2.1644238773935;-8.01448224392301;-9.00828091871894]
Robot Arm 1 end: [-7.67252995127645;-9.06034705699915;1.39656055618663]
Robot Arm 1 togoal: 25.9928957599417
Mobile Platform 2 Pose: [3.65855418225612;6.16186243096791;6.98629869248573]
Robot Arm 2 end: [6.56571332490918;7.0491758371433;1.24953025943865]
Robot Arm 1 togoal: 23.773324347027

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.362477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 25:
Mobile Platform 1 Pose: [-2.44626085919039;-7.76454722781176;-9.00258111859517]
Robot Arm 1 end: [-7.61943120739211;-9.03021772187077;1.46464438458835]
Robot Arm 1 togoal: 25.9344696990167
Mobile Platform 2 Pose: [3.40094856611747;6.30106147607418;6.95820073330021]
Robot Arm 2 end: [6.31878213293978;6.62038449767604;1.21446015679832]
Robot Arm 1 togoal: 23.2945014505229

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.619190e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 26:
Mobile Platform 1 Pose: [-2.7510219065989;-7.51735258932454;-8.96523395399497]
Robot Arm 1 end: [-7.33732300209739;-9.09874725931447;1.44437787371561]
Robot Arm 1 togoal: 25.7943944822216
Mobile Platform 2 Pose: [3.45425052269045;6.2107387980806;6.98803370681912]
Robot Arm 2 end: [5.56114813294979;7.07351205230672;1.01199181145553]
Robot Arm 1 togoal: 23.1065625819133

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.373118e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 27:
Mobile Platform 1 Pose: [-3.04630967608699;-7.2674978382532;-8.97375532574021]
Robot Arm 1 end: [-7.01376440736102;-9.08721170646353;1.43543341130051]
Robot Arm 1 togoal: 25.5694573989681
Mobile Platform 2 Pose: [4.63276182917926;5.62148314349654;6.98803370607984]
Robot Arm 2 end: [5.10725153249655;6.87726418003835;1.18690612726856]
Robot Arm 1 togoal: 22.653536452058

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 28:
Mobile Platform 1 Pose: [-3.04059844193584;-7.06972646472629;-8.82082892574942]
Robot Arm 1 end: [-6.76372787913102;-8.93428402919637;1.41772347602303]
Robot Arm 1 togoal: 25.2891350216697
Mobile Platform 2 Pose: [5.26761147020997;5.14494458931673;6.99751377852188]
Robot Arm 2 end: [5.627178003755;6.70326234477824;0.0524232101665509]
Robot Arm 1 togoal: 22.9208285868371

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.375780e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 29:
Mobile Platform 1 Pose: [-2.55606609137714;-7.22445498455319;-8.62446425185528]
Robot Arm 1 end: [-6.72887304752269;-8.72584099451487;1.31686592144031]
Robot Arm 1 togoal: 25.1108340186863
Mobile Platform 2 Pose: [5.11039631287331;5.18841833058519;6.96310533497734]
Robot Arm 2 end: [5.58120377840648;6.86730236764616;-0.0343889908468496]
Robot Arm 1 togoal: 23.0151914017389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 30:
Mobile Platform 1 Pose: [-2.56691396852799;-7.21878248162285;-8.62620902642605]
Robot Arm 1 end: [-6.88634032365575;-8.68088083989489;1.43224023369134]
Robot Arm 1 togoal: 25.1819712647122
Mobile Platform 2 Pose: [5.05176479398341;5.16183748646803;6.91740159788646]
Robot Arm 2 end: [4.59593133555068;7.09887172825604;0.771897930468041]
Robot Arm 1 togoal: 22.4938609367543

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.551505e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 31:
Mobile Platform 1 Pose: [-2.25261899103335;-7.30251805234359;-8.39064936477699]
Robot Arm 1 end: [-6.92392396857748;-8.6162971196544;1.34942360537395]
Robot Arm 1 togoal: 25.1597925150554
Mobile Platform 2 Pose: [5.59090252612838;4.93000756539299;6.8238240033754]
Robot Arm 2 end: [4.30813702163381;6.86934205098275;0.834268696115727]
Robot Arm 1 togoal: 22.1307271942573

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.400066e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 32:
Mobile Platform 1 Pose: [-1.73396564491366;-7.47979641122963;-8.21437576076699]
Robot Arm 1 end: [-7.29225737356401;-8.51557859533042;1.40279998558642]
Robot Arm 1 togoal: 25.3350110921003
Mobile Platform 2 Pose: [6.15438285079619;4.70055488237231;6.72456939945955]
Robot Arm 2 end: [4.07790816151653;6.62647185789369;0.925517742318136]
Robot Arm 1 togoal: 21.7940740515544

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.460635e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 33:
Mobile Platform 1 Pose: [-1.37697988945103;-7.62223729652951;-8.00892359067592]
Robot Arm 1 end: [-7.62232629521887;-8.31967459455795;1.42062726457138]
Robot Arm 1 togoal: 25.4198138465189
Mobile Platform 2 Pose: [6.15633669244197;4.71510621339233;6.71273740767726]
Robot Arm 2 end: [4.07380880112868;6.55534553167187;0.935188753264022]
Robot Arm 1 togoal: 21.7369440046336

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.278269e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 34:
Mobile Platform 1 Pose: [-1.34022426395381;-7.66684484137332;-7.76293544676294]
Robot Arm 1 end: [-7.86937550860135;-7.98281675198918;1.41457966052583]
Robot Arm 1 togoal: 25.3516785379524
Mobile Platform 2 Pose: [6.27996134186819;4.64931973940568;6.69820904799538]
Robot Arm 2 end: [4.41194551718635;6.26842612770085;1.32090913082127]
Robot Arm 1 togoal: 21.7349004740932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.332873e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 35:
Mobile Platform 1 Pose: [-1.67496099211943;-7.48206729147939;-7.59453931540816]
Robot Arm 1 end: [-7.85767614805135;-7.58983787169413;1.36003107043864]
Robot Arm 1 togoal: 25.0664098896801
Mobile Platform 2 Pose: [6.86481473402486;4.27535285237008;6.5201116750491]
Robot Arm 2 end: [4.14930130977764;6.13286090702831;1.39845400364447]
Robot Arm 1 togoal: 21.4589595768672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.423469e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 36:
Mobile Platform 1 Pose: [-1.98180231053834;-7.23500982518486;-7.63278355790208]
Robot Arm 1 end: [-7.72976000321264;-7.54760947182788;1.2451787167036]
Robot Arm 1 togoal: 24.9467245410619
Mobile Platform 2 Pose: [7.08162520388827;4.09071478619852;6.30968190105033]
Robot Arm 2 end: [4.05830321018936;6.01162255672267;1.46182058324928]
Robot Arm 1 togoal: 21.3075443975118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.969326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 37:
Mobile Platform 1 Pose: [-2.01526224753319;-7.1860849516721;-7.66349477419332]
Robot Arm 1 end: [-7.7746066930311;-7.54745080240105;1.123797847593]
Robot Arm 1 togoal: 24.9801409584595
Mobile Platform 2 Pose: [7.08828021258881;4.08320943685867;6.11470168309339]
Robot Arm 2 end: [4.08633116438018;5.95247575649307;1.5053290803223]
Robot Arm 1 togoal: 21.2815989533637

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.313839e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 38:
Mobile Platform 1 Pose: [-2.01173493284992;-7.17539858350307;-7.67654469376529]
Robot Arm 1 end: [-7.783530504876;-7.47302106916276;1.10296520059093]
Robot Arm 1 togoal: 24.9346180158185
Mobile Platform 2 Pose: [6.8492887664703;4.247660159048;5.92640363600019]
Robot Arm 2 end: [4.29520317906237;5.93121460652202;1.51424857201314]
Robot Arm 1 togoal: 21.4045901234178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.877347e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 39:
Mobile Platform 1 Pose: [-2.13562043661561;-7.11034598864131;-7.66186165211263]
Robot Arm 1 end: [-7.63313813056025;-7.6306160859782;1.27734866099217]
Robot Arm 1 togoal: 24.9364301750987
Mobile Platform 2 Pose: [6.59966015836686;4.41071361153228;5.8851572171573]
Robot Arm 2 end: [4.51233002130684;6.03752592826889;1.47807644984614]
Robot Arm 1 togoal: 21.6289575102079

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.257569e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 40:
Mobile Platform 1 Pose: [-2.14895249543497;-7.08771079471341;-7.67517772421489]
Robot Arm 1 end: [-7.54040412303282;-7.67741679372026;1.28796255989963]
Robot Arm 1 togoal: 24.9040387838338
Mobile Platform 2 Pose: [7.079207022243;4.10964257156955;5.92034738656967]
Robot Arm 2 end: [4.30855782348089;5.95735744320513;1.47171295393531]
Robot Arm 1 togoal: 21.4330238830413

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.993466e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 41:
Mobile Platform 1 Pose: [-2.15371947444427;-7.076477318725;-7.6836367383142]
Robot Arm 1 end: [-7.5191703925645;-7.74330478168554;1.2511653073888]
Robot Arm 1 togoal: 24.9362989752054
Mobile Platform 2 Pose: [7.50596551758205;3.79338631493515;5.94296263253629]
Robot Arm 2 end: [4.24820976188587;5.58917741113791;0.923370557057987]
Robot Arm 1 togoal: 21.1279694889429

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.169150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 42:
Mobile Platform 1 Pose: [-2.56800790228114;-6.83087577130109;-7.63694742303533]
Robot Arm 1 end: [-7.36537454024192;-7.48302391447179;1.14363837714253]
Robot Arm 1 togoal: 24.644568568171
Mobile Platform 2 Pose: [7.19680719963577;3.88954480628767;5.86132497661806]
Robot Arm 2 end: [4.37394921037851;5.69498067336673;0.571023379124965]
Robot Arm 1 togoal: 21.3036449890648

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.541139e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 43:
Mobile Platform 1 Pose: [-2.98780588244028;-6.58124656381283;-7.62333644175954]
Robot Arm 1 end: [-7.09016661370866;-7.2229007830238;1.03206138982377]
Robot Arm 1 togoal: 24.2681100877412
Mobile Platform 2 Pose: [6.58629350512055;4.22641784788322;5.75844996196497]
Robot Arm 2 end: [4.68573808332163;5.88818922972394;0.33768857176133]
Robot Arm 1 togoal: 21.6680798869591

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.572107e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 44:
Mobile Platform 1 Pose: [-3.04883268682134;-6.54074132031414;-7.62944890971374]
Robot Arm 1 end: [-7.03220691807209;-7.12929011445774;0.937258316614636]
Robot Arm 1 togoal: 24.1629759483016
Mobile Platform 2 Pose: [6.32505497911741;4.3932352047609;5.75281684669726]
Robot Arm 2 end: [4.51636062193038;6.16069639099426;-0.00948979308471803]
Robot Arm 1 togoal: 21.7769426815281

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.774121e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 45:
Mobile Platform 1 Pose: [-3.14617396816437;-6.48760039107139;-7.62092557685582]
Robot Arm 1 end: [-7.02830719359745;-7.29952454811927;1.13521845934047]
Robot Arm 1 togoal: 24.2772655558162
Mobile Platform 2 Pose: [5.95880636342706;4.61372347539858;5.75496856357877]
Robot Arm 2 end: [5.14961625999549;6.09583357484971;0.498448730888675]
Robot Arm 1 togoal: 22.127637523973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.217754e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 46:
Mobile Platform 1 Pose: [-3.40961145020342;-6.35429205599978;-7.58065588075498]
Robot Arm 1 end: [-6.82696098128852;-7.41401866089025;1.30059702953422]
Robot Arm 1 togoal: 24.2165901657733
Mobile Platform 2 Pose: [6.44219896624856;4.2527530134307;5.66141952734208]
Robot Arm 2 end: [3.99884407359528;6.00475451790551;0.246568435653629]
Robot Arm 1 togoal: 21.3012670606106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.269205e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.669720e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 47:
Mobile Platform 1 Pose: [-3.45292772817164;-6.33104979342269;-7.57466721915749]
Robot Arm 1 end: [-6.53508864788384;-7.51792393865761;1.4601233356708]
Robot Arm 1 togoal: 24.0892200305911
Mobile Platform 2 Pose: [6.8800981802508;4.02288250997096;5.56312764045083]
Robot Arm 2 end: [3.84019193631855;5.86846846474782;0.23980678649409]
Robot Arm 1 togoal: 21.0950387603618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.257627e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.321215e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 48:
Mobile Platform 1 Pose: [-3.49400942473915;-6.30437164436919;-7.5729596721314]
Robot Arm 1 end: [-6.51768004664874;-7.43223611773462;1.45012155360664]
Robot Arm 1 togoal: 24.0150293346078
Mobile Platform 2 Pose: [7.28078946056327;3.78845731281695;5.50825338272192]
Robot Arm 2 end: [3.46112862172871;5.56743010556274;0.490992927243664]
Robot Arm 1 togoal: 20.6060131619931

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.538470e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.443509e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 49:
Mobile Platform 1 Pose: [-3.90993335639315;-6.04754777599892;-7.54526268073263]
Robot Arm 1 end: [-6.11935056353302;-7.54678532196429;1.50475435206796]
Robot Arm 1 togoal: 23.8269469644403
Mobile Platform 2 Pose: [8.09555910566122;3.27194999320408;5.52631382119995]
Robot Arm 2 end: [3.15453040656632;5.04569057169001;0.241380390623085]
Robot Arm 1 togoal: 20.0262802672144

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.273084e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.016519e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 50:
Mobile Platform 1 Pose: [-4.49102142104817;-5.6829628907586;-7.40537628762713]
Robot Arm 1 end: [-5.56620851573564;-7.48578864820391;1.50669716867881]
Robot Arm 1 togoal: 23.4106782639034
Mobile Platform 2 Pose: [8.08653387119985;3.29931302217704;5.52567345468743]
Robot Arm 2 end: [3.46858005117126;5.29008312486187;0.156379834212147]
Robot Arm 1 togoal: 20.4218382805483

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.266425e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.592222e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 51:
Mobile Platform 1 Pose: [-5.96037379879653;-4.67674165149279;-7.2424798831917]
Robot Arm 1 end: [-4.64246346252614;-7.21167076919071;1.51876655879968]
Robot Arm 1 togoal: 22.5974191426076
Mobile Platform 2 Pose: [8.330945457434;3.10385159237048;5.55691476525572]
Robot Arm 2 end: [3.36087535318772;5.37155421315832;0.127667804238596]
Robot Arm 1 togoal: 20.4141873151794

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.446258e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.930574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 52:
Mobile Platform 1 Pose: [-6.73382235933833;-4.16849855156404;-7.12096106267442]
Robot Arm 1 end: [-4.06508401127162;-7.21401325886174;1.50686360849578]
Robot Arm 1 togoal: 22.2294633439038
Mobile Platform 2 Pose: [8.54604026553179;2.94745373165224;5.58202707159906]
Robot Arm 2 end: [2.6581085724244;6.19105349969566;0.235037808420572]
Robot Arm 1 togoal: 20.5920281861565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.258611e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.615846e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 53:
Mobile Platform 1 Pose: [-6.65444540120512;-4.1715799986854;-6.94192779594094]
Robot Arm 1 end: [-4.68211058406888;-6.39334064630477;0.178753123661366]
Robot Arm 1 togoal: 22.0478483173678
Mobile Platform 2 Pose: [8.47320523736221;2.99825756896632;5.53130617178124]
Robot Arm 2 end: [3.13893974342407;5.52298890321667;-0.049686042451842]
Robot Arm 1 togoal: 20.3975973270057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.437532e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.508507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 54:
Mobile Platform 1 Pose: [-6.91876366528129;-4.04045775385557;-6.89361359049823]
Robot Arm 1 end: [-4.35927475192049;-6.29246901321477;0.0948364236505446]
Robot Arm 1 togoal: 21.763898099702
Mobile Platform 2 Pose: [8.88053570111036;2.72671562180093;5.5679520338767]
Robot Arm 2 end: [2.73001073476239;5.83866995576363;0.0784809065878941]
Robot Arm 1 togoal: 20.3714874422472

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.530000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.598986e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 55:
Mobile Platform 1 Pose: [-7.45154470417685;-3.73451909801774;-6.88898207186219]
Robot Arm 1 end: [-3.97539153827367;-6.07637990291181;0.463896297797426]
Robot Arm 1 togoal: 21.3278931839975
Mobile Platform 2 Pose: [8.5647096996081;2.91804486693917;5.50508812231112]
Robot Arm 2 end: [2.95394883041967;5.10012123262377;0.00220476832781963]
Robot Arm 1 togoal: 19.9530747128561

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.629425e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.519493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 56:
Mobile Platform 1 Pose: [-7.90706838196473;-3.49226177772322;-6.82724486089986]
Robot Arm 1 end: [-3.4320921725012;-6.76128032713209;1.47455588117657]
Robot Arm 1 togoal: 21.4793802376011
Mobile Platform 2 Pose: [8.57425842900981;2.88783549164948;5.54786512133133]
Robot Arm 2 end: [2.98306079855093;5.29483542530292;-0.0963168446704384]
Robot Arm 1 togoal: 20.1272842719038

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.235543e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 57:
Mobile Platform 1 Pose: [-7.71137064297269;-3.61152121771163;-6.79194089728956]
Robot Arm 1 end: [-3.5033056833861;-6.14050883833177;1.20480844183539]
Robot Arm 1 togoal: 21.0465024895038
Mobile Platform 2 Pose: [8.67932271161013;2.80576250002217;5.57979143052138]
Robot Arm 2 end: [2.90154139403294;5.48105949660132;-0.156282582442555]
Robot Arm 1 togoal: 20.221924990975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.593962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.081252e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 58:
Mobile Platform 1 Pose: [-8.09190633280411;-3.39044830600857;-6.76175095161719]
Robot Arm 1 end: [-3.50966829367711;-7.32100696800711;1.26315073488913]
Robot Arm 1 togoal: 21.9680428357752
Mobile Platform 2 Pose: [8.80426379816186;2.7289556024657;5.60240745998289]
Robot Arm 2 end: [2.37890873638969;6.02595247053653;0.0363798667112149]
Robot Arm 1 togoal: 20.3044978029974

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.874595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.033394e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 59:
Mobile Platform 1 Pose: [-8.06433637199057;-3.38235765518789;-6.77743967077065]
Robot Arm 1 end: [-3.51194013368943;-7.58977124731051;0.670241981448879]
Robot Arm 1 togoal: 22.1967648074049
Mobile Platform 2 Pose: [8.93166555956058;2.65654811354435;5.5979359594549]
Robot Arm 2 end: [2.30405341995921;6.17278280705963;0.21767362693215]
Robot Arm 1 togoal: 20.3628901170798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.981061e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.227927e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 60:
Mobile Platform 1 Pose: [-7.92622930466151;-3.42750759670653;-6.79860692275108]
Robot Arm 1 end: [-3.6446945426999;-7.4709648787922;0.267812375372542]
Robot Arm 1 togoal: 22.2032155820299
Mobile Platform 2 Pose: [8.76340505615575;2.75887316864409;5.57753951587925]
Robot Arm 2 end: [2.39391153866445;6.11209639955729;0.357034892247]
Robot Arm 1 togoal: 20.3608327037215

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.251339e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.814054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 61:
Mobile Platform 1 Pose: [-7.75938552616627;-3.46917060712684;-6.82392203343726]
Robot Arm 1 end: [-3.75063212812185;-7.09674537337499;0.00202966997884441]
Robot Arm 1 togoal: 21.9928592308604
Mobile Platform 2 Pose: [8.68719912162391;2.81030902176929;5.55605792492146]
Robot Arm 2 end: [2.48513454064913;5.93609689344523;0.455793336708235]
Robot Arm 1 togoal: 20.2724550543565

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.944855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.467452e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 62:
Mobile Platform 1 Pose: [-8.0259210117684;-3.19794060702879;-6.82663822617844]
Robot Arm 1 end: [-2.89053841061224;-6.92855170815557;1.37989704642759]
Robot Arm 1 togoal: 21.2781990190588
Mobile Platform 2 Pose: [8.40480626501737;2.98428800472634;5.52959976210529]
Robot Arm 2 end: [3.39986537537019;5.67225480566117;0.240290767417026]
Robot Arm 1 togoal: 20.6595299779554

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.440137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.483221e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 63:
Mobile Platform 1 Pose: [-7.98607233318023;-3.23338227092209;-6.80267906231532]
Robot Arm 1 end: [-3.18253259842272;-6.71655406201837;1.47863131886706]
Robot Arm 1 togoal: 21.2890615228103
Mobile Platform 2 Pose: [7.57468733666275;3.3901701357238;5.37881016196431]
Robot Arm 2 end: [2.75920794189087;5.47563605414363;0.658299718760804]
Robot Arm 1 togoal: 20.0757801930578

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.297107e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.017671e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 64:
Mobile Platform 1 Pose: [-7.7089576530836;-3.39246791055918;-6.59549421785138]
Robot Arm 1 end: [-3.50929618662023;-6.42365813493348;1.42906600808312]
Robot Arm 1 togoal: 21.26607819627
Mobile Platform 2 Pose: [7.37571192849456;3.53965134283786;5.35281174963546]
Robot Arm 2 end: [3.40542825410096;5.71557602760351;0.175093023740236]
Robot Arm 1 togoal: 20.7001522251403

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.225785e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.353033e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 65:
Mobile Platform 1 Pose: [-7.7089576530836;-3.39246791055918;-6.2419408272581]
Robot Arm 1 end: [-3.5566007306822;-5.79006693087;1.30352442612856]
Robot Arm 1 togoal: 20.8123745083957
Mobile Platform 2 Pose: [7.24493465793127;3.62142960695724;5.32980728740384]
Robot Arm 2 end: [3.67524894731988;5.61474341801565;0.051474642865837]
Robot Arm 1 togoal: 20.8084634357488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.484319e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.827963e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 66:
Mobile Platform 1 Pose: [-8.50974180451972;-2.95413279639004;-6.12479277331336]
Robot Arm 1 end: [-3.10512373360372;-5.59461689149841;1.28650573179775]
Robot Arm 1 togoal: 20.3713360295168
Mobile Platform 2 Pose: [7.01780884919866;3.75302907791651;5.256607358986]
Robot Arm 2 end: [3.64536256136797;5.0214974713463;-0.0171035480189149]
Robot Arm 1 togoal: 20.352077737599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.664371e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.591750e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 67:
Mobile Platform 1 Pose: [-8.48043045116558;-2.97758187907901;-5.780032788732]
Robot Arm 1 end: [-2.5751530017836;-6.00697900235078;-0.0702661760617936]
Robot Arm 1 togoal: 20.4178837748441
Mobile Platform 2 Pose: [7.61636117336553;3.30731456912193;5.13549985146139]
Robot Arm 2 end: [3.26005596608814;5.0699232384792;-0.0876484892169091]
Robot Arm 1 togoal: 20.1375127709284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.959495e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 68:
Mobile Platform 1 Pose: [-8.54159155452604;-2.93397533025276;-5.78870641301374]
Robot Arm 1 end: [-2.31123580767004;-5.85622567213461;0.139384823990162]
Robot Arm 1 togoal: 20.1220098442551
Mobile Platform 2 Pose: [8.09890034167973;2.96852934012637;5.07401055355796]
Robot Arm 2 end: [3.18630836158262;5.17518666013805;0.978799514928903]
Robot Arm 1 togoal: 20.1111660463233

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.495790e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.620072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 69:
Mobile Platform 1 Pose: [-8.97834677591824;-2.61815090516741;-5.78812625289898]
Robot Arm 1 end: [-2.40960868455178;-5.65139151333728;1.35143967034147]
Robot Arm 1 togoal: 19.9748140094686
Mobile Platform 2 Pose: [8.15158298842892;2.94315513728269;5.09702069107098]
Robot Arm 2 end: [3.21055134463778;5.38739477239388;0.886796255270236]
Robot Arm 1 togoal: 20.2902143680306

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.854200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.500634e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 70:
Mobile Platform 1 Pose: [-9.27887681976277;-2.39782172165622;-5.77229971377205]
Robot Arm 1 end: [-1.67839242722734;-5.90539053038707;0.294922707219617]
Robot Arm 1 togoal: 19.7704214203016
Mobile Platform 2 Pose: [8.37014487344733;2.80002767921586;5.07838242563316]
Robot Arm 2 end: [3.09432177230764;5.48431274638412;0.766257401194436]
Robot Arm 1 togoal: 20.2927287542857

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.156285e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.081153e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 71:
Mobile Platform 1 Pose: [-9.57510499430742;-2.07750561977697;-5.72155346188086]
Robot Arm 1 end: [-1.31788827398407;-5.63544915895027;0.487087533525152]
Robot Arm 1 togoal: 19.3295328544907
Mobile Platform 2 Pose: [8.72592285311791;2.58416033841768;5.07280747021273]
Robot Arm 2 end: [2.40864821561197;5.56193771584942;0.154444540417163]
Robot Arm 1 togoal: 19.9503365845016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.429606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.162939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 72:
Mobile Platform 1 Pose: [-11.5795474373398;-0.827505055944368;-5.6947102087728]
Robot Arm 1 end: [-0.213353040724654;-6.1420260305397;0.610415510400383]
Robot Arm 1 togoal: 19.1234606914596
Mobile Platform 2 Pose: [8.89860526679854;2.48518738434395;5.05260344429486]
Robot Arm 2 end: [2.58177985946918;5.51453596272339;0.195194020973764]
Robot Arm 1 togoal: 20.0189852905886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.092886e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 73:
Mobile Platform 1 Pose: [-11.0253884695865;-1.03221623579809;-5.61541990547141]
Robot Arm 1 end: [-1.23384429162315;-5.05743400962869;0.073242097047092]
Robot Arm 1 togoal: 18.842016332344
Mobile Platform 2 Pose: [8.86641486847021;2.46913146680678;4.84811433488212]
Robot Arm 2 end: [2.77320919686411;4.72633982420805;0.135524696283521]
Robot Arm 1 togoal: 19.5432724900954

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.950080e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 74:
Mobile Platform 1 Pose: [-11.4566228870248;-0.789554457080533;-5.57092653712634]
Robot Arm 1 end: [-0.0739395155327781;-5.71025227006984;0.363034115785346]
Robot Arm 1 togoal: 18.69858734613
Mobile Platform 2 Pose: [8.91357060114627;2.40673647545524;4.80401902431116]
Robot Arm 2 end: [2.49087194625049;4.50804567858614;0.577773621669286]
Robot Arm 1 togoal: 19.1675493318877

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.672775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.340363e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 75:
Mobile Platform 1 Pose: [-11.758619580415;-0.508297596649852;-5.52362774538579]
Robot Arm 1 end: [0.268615171588966;-5.45837168100354;0.551889403822842]
Robot Arm 1 togoal: 18.2920824690439
Mobile Platform 2 Pose: [8.76535751857689;2.58452065855926;4.62749626425486]
Robot Arm 2 end: [2.7762080684452;4.39279169936627;0.47975801772626]
Robot Arm 1 togoal: 19.2735085123389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.912823e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.568194e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 76:
Mobile Platform 1 Pose: [-12.940035706331;0.155419424828432;-5.49663492087543]
Robot Arm 1 end: [0.0211074809033648;-6.26822555672661;0.731190579468527]
Robot Arm 1 togoal: 19.1012370697025
Mobile Platform 2 Pose: [8.46635499347908;2.78776739626174;4.48192480105248]
Robot Arm 2 end: [2.88219531884907;4.15221155314659;0.451702958344726]
Robot Arm 1 togoal: 19.1671449162698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.279893e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.305409e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 77:
Mobile Platform 1 Pose: [-13.0101498838501;0.167953924512448;-5.49916360231626]
Robot Arm 1 end: [-0.0155533870501566;-6.26416798653575;0.592112884817223]
Robot Arm 1 togoal: 19.1232136738535
Mobile Platform 2 Pose: [8.41980955742393;2.78461741431227;4.21991201779135]
Robot Arm 2 end: [2.7604979263548;3.49122783638489;0.713930902540464]
Robot Arm 1 togoal: 18.5874930098124

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.144756e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.347493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 78:
Mobile Platform 1 Pose: [-12.9260545270535;0.124710556510208;-5.54965827000878]
Robot Arm 1 end: [0.5872864097833;-6.06744119889258;0.610925726419396]
Robot Arm 1 togoal: 18.6437678325695
Mobile Platform 2 Pose: [8.39250373897436;2.77496986745819;4.25971577755635]
Robot Arm 2 end: [3.45082045491622;4.41784985187814;0.448128207663624]
Robot Arm 1 togoal: 19.747152178086

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.227307e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 79:
Mobile Platform 1 Pose: [-12.5554943096241;-0.0575112024358243;-5.50125363537301]
Robot Arm 1 end: [0.111598355577098;-5.54849735652966;-0.0961303044605107]
Robot Arm 1 togoal: 18.4973340659956
Mobile Platform 2 Pose: [8.46511292105785;2.70946015102863;4.24197179880727]
Robot Arm 2 end: [3.29571631366419;4.45678808352667;0.285733248070736]
Robot Arm 1 togoal: 19.6799562482709

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.902776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 80:
Mobile Platform 1 Pose: [-12.6585496540167;0.0517541534929246;-5.39269348650126]
Robot Arm 1 end: [0.504141811919728;-5.44992354194157;1.21355146786175]
Robot Arm 1 togoal: 18.1374194630848
Mobile Platform 2 Pose: [9.17960643754168;2.3758552282998;4.33636717297327]
Robot Arm 2 end: [2.5535198115189;4.00169413951643;0.0321601330938125]
Robot Arm 1 togoal: 18.864117858624

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.939230e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.975261e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 81:
Mobile Platform 1 Pose: [-13.0184066513431;0.301429626511994;-5.3799593523513]
Robot Arm 1 end: [0.815546265586693;-5.29596739052187;1.14437816848034]
Robot Arm 1 togoal: 17.8455219261154
Mobile Platform 2 Pose: [9.25891828433336;2.29053301941475;4.33266792335658]
Robot Arm 2 end: [2.5411502346943;3.9687428476851;0.074569106453411]
Robot Arm 1 togoal: 18.8281275830533

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.729410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.366399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 82:
Mobile Platform 1 Pose: [-12.8170997193495;0.23254177268264;-5.30600732832799]
Robot Arm 1 end: [0.803570746976771;-5.30517462913362;1.07882191697186]
Robot Arm 1 togoal: 17.8610806753468
Mobile Platform 2 Pose: [9.33940382785016;2.19904883295351;4.30871725146894]
Robot Arm 2 end: [2.88229802215959;4.15616290433021;0.891868036915496]
Robot Arm 1 togoal: 19.1506229019488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.703363e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.751647e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 83:
Mobile Platform 1 Pose: [-12.5681531657103;0.110786358399826;-5.26270173686016]
Robot Arm 1 end: [0.089308918506138;-5.18792262882719;-0.170060238040367]
Robot Arm 1 togoal: 18.2141284782095
Mobile Platform 2 Pose: [9.37850666173106;2.17865185765583;4.3138416399239]
Robot Arm 2 end: [2.88843008129946;4.03131618431417;0.705286099499287]
Robot Arm 1 togoal: 19.0697365737498

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.191827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.569262e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 84:
Mobile Platform 1 Pose: [-12.4855738539567;0.055261570962374;-5.24517047182767]
Robot Arm 1 end: [-0.123387262296907;-5.13718365647611;-0.116472678083292]
Robot Arm 1 togoal: 18.2838348134026
Mobile Platform 2 Pose: [9.29272904559864;2.21892602014681;4.30161994924373]
Robot Arm 2 end: [2.72440339940775;3.92168304236313;1.06538760481257]
Robot Arm 1 togoal: 18.8661411861973

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.253879e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.728053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 85:
Mobile Platform 1 Pose: [-12.3301885998925;-0.0578553470302717;-5.21458264150153]
Robot Arm 1 end: [-0.278155268927052;-5.24078403343349;-0.0569121286119321]
Robot Arm 1 togoal: 18.45025149887
Mobile Platform 2 Pose: [9.11238065404071;2.26977672613246;4.26061576734397]
Robot Arm 2 end: [2.00085250955217;4.11951834052719;1.40694767416702]
Robot Arm 1 togoal: 18.5309003304625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.971619e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.878540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 86:
Mobile Platform 1 Pose: [-12.2719100354995;-0.0936280955739384;-5.22634830906397]
Robot Arm 1 end: [-0.524625745169676;-5.29537249278496;-0.0298970671442517]
Robot Arm 1 togoal: 18.6311955730653
Mobile Platform 2 Pose: [9.91999036356424;1.75333771151998;4.23433918521407]
Robot Arm 2 end: [1.41053810631533;4.44962482917727;-0.00420847427440602]
Robot Arm 1 togoal: 18.4748017166531

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.456663e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.034344e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 87:
Mobile Platform 1 Pose: [-12.0909619429132;-0.267142790424912;-5.19676031197396]
Robot Arm 1 end: [-0.400871965775671;-5.11433582493895;-0.0508197546887424]
Robot Arm 1 togoal: 18.4144481975084
Mobile Platform 2 Pose: [9.82420451176758;1.79857376172141;4.26983478061033]
Robot Arm 2 end: [1.24482200626775;4.45998051386205;0.282949521857591]
Robot Arm 1 togoal: 18.359461522614

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.285565e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.222671e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 88:
Mobile Platform 1 Pose: [-12.4731131530114;-0.0450946521254746;-5.08184240555085]
Robot Arm 1 end: [-0.155229018436661;-5.19347816176383;-0.0547890400488172]
Robot Arm 1 togoal: 18.3426706389127
Mobile Platform 2 Pose: [10.327611339588;1.51610974103664;4.24163925714054]
Robot Arm 2 end: [1.08489508954392;3.87350735209859;1.05544585442535]
Robot Arm 1 togoal: 17.7641731036767

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.297047e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.248624e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 89:
Mobile Platform 1 Pose: [-12.7220200057029;0.142603520647961;-4.80296107076503]
Robot Arm 1 end: [0.167333228136901;-5.47356711186072;0.895875102372689]
Robot Arm 1 togoal: 18.3440288963301
Mobile Platform 2 Pose: [10.7634927398676;1.14953548164944;4.19408362085271]
Robot Arm 2 end: [0.501838695352191;4.0947237519146;1.1917307680015]
Robot Arm 1 togoal: 17.5800532380628

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.272851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.394540e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 90:
Mobile Platform 1 Pose: [-12.5357660573579;0.0792555933369332;-4.73116532216387]
Robot Arm 1 end: [0.0495948410077441;-4.72697042502933;-0.0908968912468921]
Robot Arm 1 togoal: 17.8463562756043
Mobile Platform 2 Pose: [11.8693655864734;0.542247947175533;4.1558764822258]
Robot Arm 2 end: [0.373510333495851;3.78856969124932;1.36200024276566]
Robot Arm 1 togoal: 17.2557135955713

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.852984e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.183690e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 91:
Mobile Platform 1 Pose: [-12.835732707508;0.284130396707313;-4.74188496123336]
Robot Arm 1 end: [0.114469408591451;-4.90371756769787;-0.0539141889691282]
Robot Arm 1 togoal: 17.9534082311409
Mobile Platform 2 Pose: [12.5528347520826;0.0494875671196957;4.03027488601117]
Robot Arm 2 end: [0.622596913509546;4.48524121122192;0.277508116816985]
Robot Arm 1 togoal: 18.0057782447115

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.845249e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.996941e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 92:
Mobile Platform 1 Pose: [-12.6634880486454;0.152683642458143;-4.75501411269328]
Robot Arm 1 end: [0.0187197644877883;-4.82111651946841;-0.118535273117232]
Robot Arm 1 togoal: 17.9437879483481
Mobile Platform 2 Pose: [12.0138949463499;0.353929165532795;4.01774636773898]
Robot Arm 2 end: [1.00400806305909;3.98573891720203;0.311776389406004]
Robot Arm 1 togoal: 17.8367964683243

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.885884e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.530818e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 93:
Mobile Platform 1 Pose: [-12.5715602513329;0.0801262121391933;-4.73326855247589]
Robot Arm 1 end: [-0.128592543660655;-4.83334222304912;-0.059545020929073]
Robot Arm 1 togoal: 18.0309331701045
Mobile Platform 2 Pose: [12.2558567394197;0.206342888081598;4.03826019511217]
Robot Arm 2 end: [-0.0114882487374598;4.75378058190409;0.937668139732962]
Robot Arm 1 togoal: 17.8265163316696

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.878939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.057369e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 94:
Mobile Platform 1 Pose: [-12.4601827200788;-0.0315091738012571;-4.67861565389614]
Robot Arm 1 end: [-0.115072049423892;-4.84711700001728;-0.0602956012453832]
Robot Arm 1 togoal: 18.0347459520714
Mobile Platform 2 Pose: [12.2738740053351;0.19591633685341;4.04062823865572]
Robot Arm 2 end: [-0.00301814108160786;4.53765487256071;1.26573537272653]
Robot Arm 1 togoal: 17.6450620716156

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.560090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.103848e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 95:
Mobile Platform 1 Pose: [-12.3973672347354;-0.0844476399723948;-4.65250358947498]
Robot Arm 1 end: [-0.0588443382303826;-4.58852794455466;-0.0857501533203649]
Robot Arm 1 togoal: 17.7928958594434
Mobile Platform 2 Pose: [12.4291017957071;0.105127508728027;4.03581296325815]
Robot Arm 2 end: [0.0866364806888542;3.81323227653919;1.45888776164138]
Robot Arm 1 togoal: 17.1040778573222

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.996891e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.921774e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 96:
Mobile Platform 1 Pose: [-12.6891625142134;0.109679634392526;-4.55207841502214]
Robot Arm 1 end: [0.111697493239772;-4.30326524012628;-0.0573763284042553]
Robot Arm 1 togoal: 17.4600456708644
Mobile Platform 2 Pose: [13.0422133918992;-0.371048201103673;3.86619305152252]
Robot Arm 2 end: [-0.957543266494518;3.57873365424387;0.085738338372788]
Robot Arm 1 togoal: 16.3770573506827

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.036696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.471160e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 97:
Mobile Platform 1 Pose: [-12.7666150289993;0.152637890678374;-4.47946745050865]
Robot Arm 1 end: [0.16824750080328;-4.0437450696593;0.913943950828148]
Robot Arm 1 togoal: 17.1539689236595
Mobile Platform 2 Pose: [13.4974660833454;-0.614814305175878;3.81064853484663]
Robot Arm 2 end: [-1.21123441603102;3.42113483582511;0.21296610758579]
Robot Arm 1 togoal: 16.0959688566753

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.688504e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.337045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 98:
Mobile Platform 1 Pose: [-12.6979705211399;0.107912832715514;-4.43653804869366]
Robot Arm 1 end: [-0.454368181832525;-4.52805133448536;0.883824761899887]
Robot Arm 1 togoal: 17.9098878260809
Mobile Platform 2 Pose: [13.9920212059065;-0.876008777154968;3.74602181638114]
Robot Arm 2 end: [-1.14580127305077;3.04850448887903;0.887614915564712]
Robot Arm 1 togoal: 15.7816818228537

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.080007e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.070949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 99:
Mobile Platform 1 Pose: [-13.0305592086552;0.299295123288939;-4.39352340995688]
Robot Arm 1 end: [-0.104087431591837;-4.59196034320205;0.0854924898729811]
Robot Arm 1 togoal: 17.806700179833
Mobile Platform 2 Pose: [14.4582341318973;-1.2631023490768;3.54645553471426]
Robot Arm 2 end: [-1.59424052379365;2.80178321978051;0.720559774270241]
Robot Arm 1 togoal: 15.3356822650006

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.003481e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.643956e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 100:
Mobile Platform 1 Pose: [-13.4929979902061;0.745336270192399;-4.37008224263665]
Robot Arm 1 end: [0.134057344944781;-4.56380992974774;0.368669014207754]
Robot Arm 1 togoal: 17.6286485824488
Mobile Platform 2 Pose: [16.4464116903452;-2.69283555928283;2.97896908203412]
Robot Arm 2 end: [-2.64808735321762;3.05262004905245;1.43019044746425]
Robot Arm 1 togoal: 14.9809798106024

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.437285e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.154959e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 101:
Mobile Platform 1 Pose: [-12.3482166998868;0.156809812202351;-4.41282777474676]
Robot Arm 1 end: [0.387765612901147;-4.82555550945156;1.32703770090385]
Robot Arm 1 togoal: 17.6700245458184
Mobile Platform 2 Pose: [16.5500107532962;-2.76464876977049;2.95006665909188]
Robot Arm 2 end: [-3.17159007381376;2.29644807353104;0.991750464264065]
Robot Arm 1 togoal: 14.0751526604922

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.984899e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.641639e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 102:
Mobile Platform 1 Pose: [-12.1822615305405;0.0209760414921805;-4.38986384753152]
Robot Arm 1 end: [-0.104181904062354;-5.03775541222631;1.22030080842369]
Robot Arm 1 togoal: 18.1195750444688
Mobile Platform 2 Pose: [17.8089375538883;-3.52690435229847;2.79420331808718]
Robot Arm 2 end: [-3.57682837016947;3.22108687156868;1.18530719369857]
Robot Arm 1 togoal: 14.7026175054658

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.307881e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 103:
Mobile Platform 1 Pose: [-12.2601911287097;0.00908451151921753;-4.31275981494912]
Robot Arm 1 end: [-0.276643885165048;-4.77864805347507;0.0742858100804055]
Robot Arm 1 togoal: 18.0585389713093
Mobile Platform 2 Pose: [18.3839386761326;-3.87509067747863;2.70186838990974]
Robot Arm 2 end: [-3.88861252704343;2.88355890598813;1.28651286370702]
Robot Arm 1 togoal: 14.2614911964269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.647053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.069349e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 104:
Mobile Platform 1 Pose: [-12.3455701042706;0.00553770627434447;-4.27437324021801]
Robot Arm 1 end: [0.0420443341806499;-4.37644605646605;-0.0713710508981798]
Robot Arm 1 togoal: 17.5607154700667
Mobile Platform 2 Pose: [18.8254166591901;-4.12198288568724;2.6625361837639]
Robot Arm 2 end: [-4.0721285381283;2.77863358247832;1.32548207444288]
Robot Arm 1 togoal: 14.0879864978261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.141079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.106768e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 105:
Mobile Platform 1 Pose: [-12.2285921174722;-0.0720829180226483;-4.2298303451935]
Robot Arm 1 end: [0.0130166137227212;-4.16789162247338;-0.0737230984415492]
Robot Arm 1 togoal: 17.4072436630454
Mobile Platform 2 Pose: [19.2046156465197;-4.36617113043542;2.6089431906575]
Robot Arm 2 end: [-4.26932239989398;2.61902549283319;1.35489091483892]
Robot Arm 1 togoal: 13.8603052770602

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.746686e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.356119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 106:
Mobile Platform 1 Pose: [-12.0559256529583;-0.178923669480226;-4.17736465718258]
Robot Arm 1 end: [0.0144936263194519;-4.15428420227035;-0.0902532037645152]
Robot Arm 1 togoal: 17.3968455676337
Mobile Platform 2 Pose: [19.1877222555972;-4.38243263256948;2.55989695630219]
Robot Arm 2 end: [-4.1770937911145;1.89144065398879;0.320432152320231]
Robot Arm 1 togoal: 13.294884756992

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.220076e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.421815e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 107:
Mobile Platform 1 Pose: [-11.927918943544;-0.24918297991499;-4.13397148051931]
Robot Arm 1 end: [-0.202153272770131;-4.32550426059765;-0.131916203054213]
Robot Arm 1 togoal: 17.6645445935018
Mobile Platform 2 Pose: [19.1798517683915;-4.40249425349406;2.2303404302292]
Robot Arm 2 end: [-4.69055975608929;2.34999690909505;1.25780590246806]
Robot Arm 1 togoal: 13.4455141400875

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.056481e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.184915e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 108:
Mobile Platform 1 Pose: [-11.9467174272985;-0.231041075099464;-4.12081621365282]
Robot Arm 1 end: [-0.516119535998452;-4.52985152874653;0.0376019625164533]
Robot Arm 1 togoal: 17.9973838454492
Mobile Platform 2 Pose: [19.2764609940389;-4.43329711765948;2.08428946244431]
Robot Arm 2 end: [-4.13311772368459;1.43799371969043;0.382528986151447]
Robot Arm 1 togoal: 12.9051975662984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.279700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.194477e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 109:
Mobile Platform 1 Pose: [-11.9386793080986;-0.212681206101757;-4.12924305388113]
Robot Arm 1 end: [0.0605484038294214;-4.10592663874864;-0.031863368295561]
Robot Arm 1 togoal: 17.325739493914
Mobile Platform 2 Pose: [18.9380293323693;-4.30311610089183;2.03321172623485]
Robot Arm 2 end: [-4.62401024456898;1.5385063771078;1.1675012710728]
Robot Arm 1 togoal: 12.7343376983811

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.022112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.474328e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 110:
Mobile Platform 1 Pose: [-11.8544045720217;-0.168568604911393;-3.9099932634237]
Robot Arm 1 end: [0.306343293223048;-3.56347423527926;1.00153415201569]
Robot Arm 1 togoal: 16.6794681703929
Mobile Platform 2 Pose: [19.5366736241096;-4.60355069804764;1.91192803741811]
Robot Arm 2 end: [-4.33697616948749;1.93644235048815;-0.17296357345837]
Robot Arm 1 togoal: 13.3198351145832

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.512249e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.417781e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 111:
Mobile Platform 1 Pose: [-11.9076991615787;-0.157038332399879;-3.95815501431751]
Robot Arm 1 end: [-0.160522721652517;-3.52793502983321;0.811487790349512]
Robot Arm 1 togoal: 16.9335364210861
Mobile Platform 2 Pose: [19.1775600310037;-4.353579246313;1.91570670223145]
Robot Arm 2 end: [-4.1794337494364;1.91570988884833;-0.214154262046652]
Robot Arm 1 togoal: 13.3743745253936

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.929668e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.475467e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 112:
Mobile Platform 1 Pose: [-12.9645384212132;0.407882910209447;-3.97633387935832]
Robot Arm 1 end: [0.416174544225064;-3.52926787230264;0.920203877268714]
Robot Arm 1 togoal: 16.5907129294786
Mobile Platform 2 Pose: [18.3351136660183;-3.84434064530609;1.79141294576329]
Robot Arm 2 end: [-3.97212812563826;1.45965749877079;0.0921763033590168]
Robot Arm 1 togoal: 13.0269154629535

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.156133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.153689e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 113:
Mobile Platform 1 Pose: [-13.9024733934175;0.9490232952463;-3.98074407477904]
Robot Arm 1 end: [1.19722669828589;-3.51176389138922;0.900157701686315]
Robot Arm 1 togoal: 16.1382163332072
Mobile Platform 2 Pose: [17.8946174733635;-3.64765383111135;1.94688828611706]
Robot Arm 2 end: [-4.12164363783911;1.77049180231719;0.0069683752771183]
Robot Arm 1 togoal: 13.2435585304568

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.703150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.463877e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 114:
Mobile Platform 1 Pose: [-14.7242146482611;1.4234132372164;-3.97767624010298]
Robot Arm 1 end: [1.89805616670605;-3.59231807536934;0.829930041758526]
Robot Arm 1 togoal: 15.8388819752862
Mobile Platform 2 Pose: [18.0231179724697;-3.73690799597424;1.94769168664597]
Robot Arm 2 end: [-4.57765692312759;2.00189780938056;0.72269034266711]
Robot Arm 1 togoal: 13.1941143613018

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.990111e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.171550e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 115:
Mobile Platform 1 Pose: [-15.7075257764305;1.97362885344355;-3.97680649990579]
Robot Arm 1 end: [2.62417996935339;-3.76549045096255;0.742121898452935]
Robot Arm 1 togoal: 15.6364350021704
Mobile Platform 2 Pose: [18.129633883073;-3.80553197589087;1.9513785321801]
Robot Arm 2 end: [-4.47338706632285;1.86005730179976;1.15564414774789]
Robot Arm 1 togoal: 13.089610178284

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.028086e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.760302e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 116:
Mobile Platform 1 Pose: [-16.6754343748357;2.51044238508997;-3.9808040998767]
Robot Arm 1 end: [3.00669509586626;-4.42704914359977;0.995112912694035]
Robot Arm 1 togoal: 16.041278555733
Mobile Platform 2 Pose: [18.747883728706;-4.1847689357528;1.93486869064324]
Robot Arm 2 end: [-4.66232739065059;1.65313402675267;1.24108404598787]
Robot Arm 1 togoal: 12.8204771545902

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.056244e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 117:
Mobile Platform 1 Pose: [-16.6599301217602;2.4860745473896;-3.98440961168037]
Robot Arm 1 end: [2.83410709527517;-4.35005820607205;1.28755210404135]
Robot Arm 1 togoal: 16.0414678118858
Mobile Platform 2 Pose: [19.4384521717427;-4.66843000150706;1.91703270576209]
Robot Arm 2 end: [-5.31247904823935;2.16418280582483;1.07701965307532]
Robot Arm 1 togoal: 13.0436695203142

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.243776e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.787998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 118:
Mobile Platform 1 Pose: [-16.4644695493942;2.3280736790999;-3.97857898744167]
Robot Arm 1 end: [2.50013313244774;-4.09112092498572;1.47434498012786]
Robot Arm 1 togoal: 15.9627650688863
Mobile Platform 2 Pose: [19.1729716855271;-4.50855460349565;1.90954967015493]
Robot Arm 2 end: [-5.09872402946308;2.05758347396221;1.24005396805507]
Robot Arm 1 togoal: 13.0187079252818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.910952e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.744594e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 119:
Mobile Platform 1 Pose: [-16.1938398834465;2.14107723294053;-3.9705879051211]
Robot Arm 1 end: [2.18042700418878;-3.93803671221549;1.51387628709368]
Robot Arm 1 togoal: 15.981697030515
Mobile Platform 2 Pose: [19.0832878842947;-4.44752190937081;1.89374288006067]
Robot Arm 2 end: [-4.92788968951802;1.75545467689772;1.29995791707522]
Robot Arm 1 togoal: 12.804916136575

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.848359e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.005240e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 120:
Mobile Platform 1 Pose: [-15.8496447317026;1.91971563636451;-3.85439652158884]
Robot Arm 1 end: [2.02327361399653;-3.73671244543356;1.49989735219361]
Robot Arm 1 togoal: 15.8847687413899
Mobile Platform 2 Pose: [18.9226478589541;-4.34966392108861;1.89317892284748]
Robot Arm 2 end: [-4.56258062743303;1.73278114200445;1.43823456771334]
Robot Arm 1 togoal: 12.9317644919834

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.397528e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.810615e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 121:
Mobile Platform 1 Pose: [-15.9705082070673;2.01640641665627;-3.53710217360498]
Robot Arm 1 end: [2.06522570031713;-3.52550453882283;1.51382113327255]
Robot Arm 1 togoal: 15.6811979055413
Mobile Platform 2 Pose: [19.0722092025988;-4.38955232439749;1.77131620162057]
Robot Arm 2 end: [-4.49872838077004;1.5339977802836;1.44082534757312]
Robot Arm 1 togoal: 12.7790266530368

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.819801e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 122:
Mobile Platform 1 Pose: [-16.0505165353515;2.06131361343523;-3.40242973375047]
Robot Arm 1 end: [2.29332145417922;-2.63057745189561;0.534029388162241]
Robot Arm 1 togoal: 14.8289747442492
Mobile Platform 2 Pose: [19.3412399328341;-4.47497351064456;1.55377964267062]
Robot Arm 2 end: [-4.69240072421462;1.38377040346233;-0.0552182058368438]
Robot Arm 1 togoal: 12.6588033598549

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.019550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.414294e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 123:
Mobile Platform 1 Pose: [-14.9688414680735;1.4914252074851;-3.3929058248684]
Robot Arm 1 end: [1.23773279854577;-2.74429165850421;0.176517601553324]
Robot Arm 1 togoal: 15.5242368349118
Mobile Platform 2 Pose: [19.2056842294999;-4.38713420327556;1.56254767582082]
Robot Arm 2 end: [-4.78612717028929;1.50379111538084;0.0307904841752711]
Robot Arm 1 togoal: 12.7177987211532

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.572389e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.556933e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 124:
Mobile Platform 1 Pose: [-14.8725682018724;1.43661662200233;-3.39492770712658]
Robot Arm 1 end: [1.21845858588091;-2.75803037473561;0.109342859005095]
Robot Arm 1 togoal: 15.5523498074821
Mobile Platform 2 Pose: [19.0624016576745;-4.29284518870929;1.5595085435691]
Robot Arm 2 end: [-4.51071620748928;1.30735927912089;0.0276692606496954]
Robot Arm 1 togoal: 12.657750476247

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.013892e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.486736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 125:
Mobile Platform 1 Pose: [-14.8681205372212;1.42861426527509;-3.36874519076513]
Robot Arm 1 end: [1.26531574694551;-2.65198587138418;0.247195497382371]
Robot Arm 1 togoal: 15.4269255978355
Mobile Platform 2 Pose: [18.8160694817788;-4.13935643606998;1.57982958561103]
Robot Arm 2 end: [-4.1272677706858;1.35200639865646;0.111962291896779]
Robot Arm 1 togoal: 12.8585543656828

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.160657e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 126:
Mobile Platform 1 Pose: [-15.2946321422531;1.65683314864095;-3.33160344996816]
Robot Arm 1 end: [1.88468794042288;-2.63801649861677;1.05249474646354]
Robot Arm 1 togoal: 15.0265514345847
Mobile Platform 2 Pose: [19.0009247735259;-4.30830992686276;1.59099392976831]
Robot Arm 2 end: [-4.90137434860611;2.09757070577572;0.702364222580685]
Robot Arm 1 togoal: 13.1536065415674

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.720947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.950862e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 127:
Mobile Platform 1 Pose: [-16.0090946696895;2.00103488514738;-3.28364607246535]
Robot Arm 1 end: [2.56651081505911;-3.45984691433011;1.16808205666487]
Robot Arm 1 togoal: 15.3801427657406
Mobile Platform 2 Pose: [19.0009247735569;-4.30830993152242;1.2374405439991]
Robot Arm 2 end: [-4.76819002659581;1.86702269749534;0.822434701362676]
Robot Arm 1 togoal: 12.9879196477781

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.244131e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 128:
Mobile Platform 1 Pose: [-16.3194154825356;2.24495557342891;-3.24080232487289]
Robot Arm 1 end: [2.44488687621634;-2.71231824423868;-0.0098539409882144]
Robot Arm 1 togoal: 14.8669527222162
Mobile Platform 2 Pose: [18.0696712987386;-3.76850584596548;1.16326319585131]
Robot Arm 2 end: [-4.04069503055292;1.8666750043609;0.95197870144442]
Robot Arm 1 togoal: 13.2911653595992

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.487266e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.329575e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 129:
Mobile Platform 1 Pose: [-16.7405670086003;2.47988396360347;-3.2357230929606]
Robot Arm 1 end: [3.16401697657333;-2.77509066265793;0.451406126639183]
Robot Arm 1 togoal: 14.5285111552987
Mobile Platform 2 Pose: [17.1100427979107;-3.24691019789135;1.09759835550641]
Robot Arm 2 end: [-2.53458046622135;1.04009991121977;1.0418736628546]
Robot Arm 1 togoal: 13.3358860565013

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.495013e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.074017e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 130:
Mobile Platform 1 Pose: [-17.189825221986;2.72675848105356;-3.19631530459933]
Robot Arm 1 end: [3.51391769286061;-2.91653380811543;0.593960128505859]
Robot Arm 1 togoal: 14.4832838898682
Mobile Platform 2 Pose: [17.1506806722594;-3.26516512646174;1.10387892785557]
Robot Arm 2 end: [-2.51060625875886;0.948952216161924;1.00046399009919]
Robot Arm 1 togoal: 13.275599081802

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.571798e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.715267e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 131:
Mobile Platform 1 Pose: [-17.7781349088935;3.08312347016664;-3.13869059081359]
Robot Arm 1 end: [3.18936394493356;-3.27120629599633;1.39177317601752]
Robot Arm 1 togoal: 14.917318110086
Mobile Platform 2 Pose: [17.1182799394219;-3.24941837283067;1.09947392219462]
Robot Arm 2 end: [-2.72118805593865;0.7831415325358;1.2505545987648]
Robot Arm 1 togoal: 13.0127009242032

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.738081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.977838e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 132:
Mobile Platform 1 Pose: [-17.6339232972929;2.97807668346882;-3.14159265519057]
Robot Arm 1 end: [2.95739346484166;-3.28635938782203;1.40858206889722]
Robot Arm 1 togoal: 15.0378951446888
Mobile Platform 2 Pose: [16.9731830659325;-3.18883602116639;1.06652984108707]
Robot Arm 2 end: [-2.76846810141829;0.757127804452187;1.33028056948551]
Robot Arm 1 togoal: 12.9633030552672

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.107409e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.687406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 133:
Mobile Platform 1 Pose: [-17.6896416790395;3.02012835017087;-3.13395795030241]
Robot Arm 1 end: [3.3695664040421;-3.18676305333176;1.37854320343431]
Robot Arm 1 togoal: 14.7605441442384
Mobile Platform 2 Pose: [16.8085003202114;-3.11001904858791;1.01806504307462]
Robot Arm 2 end: [-2.8504880261494;0.717959214203594;1.39275972879723]
Robot Arm 1 togoal: 12.8843555038164

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.200388e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 134:
Mobile Platform 1 Pose: [-17.8509837381188;3.17230561691739;-3.06166458947246]
Robot Arm 1 end: [3.43304407144839;-2.4607864943485;0.161361439348811]
Robot Arm 1 togoal: 14.1507853930758
Mobile Platform 2 Pose: [16.6891903218214;-3.05656212856615;0.946364256632622]
Robot Arm 2 end: [-2.99345294031962;0.665137064279695;1.41114341154123]
Robot Arm 1 togoal: 12.7612271654969

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.166277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.161143e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 135:
Mobile Platform 1 Pose: [-18.2335267625852;3.39482717930978;-3.036896652785]
Robot Arm 1 end: [3.98638648654237;-2.59078080811691;0.980350053009044]
Robot Arm 1 togoal: 13.9636532187127
Mobile Platform 2 Pose: [16.7613152102736;-3.11340134257437;0.844144967183961]
Robot Arm 2 end: [-3.21703020519438;0.648259324325272;1.39589271142517]
Robot Arm 1 togoal: 12.6257577083112

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.994653e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.384346e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 136:
Mobile Platform 1 Pose: [-19.2370538910148;3.94908599568535;-2.98440140072058]
Robot Arm 1 end: [4.71820308977274;-2.73337432611816;0.818506267401434]
Robot Arm 1 togoal: 13.8032495367675
Mobile Platform 2 Pose: [16.9868409660856;-3.24121247691497;0.746878749183467]
Robot Arm 2 end: [-3.66781955204387;1.35039246899774;0.773944712393948]
Robot Arm 1 togoal: 13.018679273586

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.467741e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.090346e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 137:
Mobile Platform 1 Pose: [-19.7019049988724;4.33619272424157;-2.79098363343869]
Robot Arm 1 end: [4.27666841158531;-2.68674418625325;1.42500802928676]
Robot Arm 1 togoal: 13.9183051043156
Mobile Platform 2 Pose: [17.4386038931858;-3.49063418661241;0.729874158613774]
Robot Arm 2 end: [-3.85156279232188;1.39223212665793;0.684985060857394]
Robot Arm 1 togoal: 12.9724767836712

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.026021e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.850133e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 138:
Mobile Platform 1 Pose: [-20.135032641714;4.57002829253107;-2.74675747749221]
Robot Arm 1 end: [4.99727100704227;-2.5767827955525;1.27701680215065]
Robot Arm 1 togoal: 13.5374403291138
Mobile Platform 2 Pose: [17.976512718707;-3.78579930457398;0.671133783432814]
Robot Arm 2 end: [-4.06234200277533;1.06752009149056;0.120021301167975]
Robot Arm 1 togoal: 12.6375838189954

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.186372e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.209020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 139:
Mobile Platform 1 Pose: [-21.0051000531419;5.09116002022741;-2.67890517351314]
Robot Arm 1 end: [5.91086332587839;-2.58202460574638;0.729316129458584]
Robot Arm 1 togoal: 13.2534955868303
Mobile Platform 2 Pose: [17.8199048902273;-3.6893285095693;0.6470495803895]
Robot Arm 2 end: [-4.02586446093191;0.909020497899454;0.133377395545885]
Robot Arm 1 togoal: 12.5148919354159

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.729414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.392191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 140:
Mobile Platform 1 Pose: [-21.8321436841776;5.61012213702781;-2.63099348019412]
Robot Arm 1 end: [5.86439065024962;-2.73201231941154;1.3771877134113]
Robot Arm 1 togoal: 13.3876093597462
Mobile Platform 2 Pose: [17.6162979977618;-3.55675299559756;0.614141350173178]
Robot Arm 2 end: [-3.88737672284245;0.70158446952515;0.0964987331501285]
Robot Arm 1 togoal: 12.4063402075748

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.402544e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.340977e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 141:
Mobile Platform 1 Pose: [-21.8146939168514;5.59731756831186;-2.63272071466367]
Robot Arm 1 end: [5.89740464377864;-2.51846862382161;1.39183713020787]
Robot Arm 1 togoal: 13.1742183217612
Mobile Platform 2 Pose: [17.6611012359188;-3.57952171253393;0.601041234008441]
Robot Arm 2 end: [-3.92136049529949;0.812575468363507;0.124858124982846]
Robot Arm 1 togoal: 12.4824219854297

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.084766e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 142:
Mobile Platform 1 Pose: [-22.024580876678;5.73800807044964;-2.60408662424754]
Robot Arm 1 end: [6.49150398850089;-2.21342653892634;0.558780226178125]
Robot Arm 1 togoal: 12.7437513698906
Mobile Platform 2 Pose: [17.3751839069092;-3.38536063017158;0.54178163536862]
Robot Arm 2 end: [-3.64289198823519;0.696198819973849;0.0367278403426146]
Robot Arm 1 togoal: 12.5309439115171

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.280768e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.073442e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 143:
Mobile Platform 1 Pose: [-22.4999740271111;6.04027593493454;-2.53490007650266]
Robot Arm 1 end: [6.69745907466646;-2.40559536826244;1.12929339764298]
Robot Arm 1 togoal: 12.843636831789
Mobile Platform 2 Pose: [17.3691153085665;-3.38050575149303;0.190048824282585]
Robot Arm 2 end: [-3.29318009719779;-0.461313055642124;0.539528216170807]
Robot Arm 1 togoal: 11.701763486481

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.305456e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.099915e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 144:
Mobile Platform 1 Pose: [-23.2351685305842;6.54086822622242;-2.4194964727011]
Robot Arm 1 end: [6.03446103942644;-2.63484380995488;-0.00801538398315532]
Robot Arm 1 togoal: 13.3305163213673
Mobile Platform 2 Pose: [17.1894177974971;-3.35606530618364;0.248642298400285]
Robot Arm 2 end: [-2.69867126283516;0.418837298682112;0.705339749310005]
Robot Arm 1 togoal: 12.7486028108618

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.985422e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.802697e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 145:
Mobile Platform 1 Pose: [-22.9633212719138;6.38332629746326;-2.47800105362986]
Robot Arm 1 end: [6.16649059816096;-2.37801401746259;-0.192746027608272]
Robot Arm 1 togoal: 13.0708817911276
Mobile Platform 2 Pose: [16.0830047938296;-2.78122931485687;0.227012808907256]
Robot Arm 2 end: [-2.18137392184123;0.427878611491207;0.802203533280541]
Robot Arm 1 togoal: 13.0532844313846

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.829353e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.483841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 146:
Mobile Platform 1 Pose: [-23.1018645999967;6.44240009133335;-2.40407219938054]
Robot Arm 1 end: [6.56297375713845;-2.09655135431368;-0.11827983716635]
Robot Arm 1 togoal: 12.6817562839777
Mobile Platform 2 Pose: [15.3837630691015;-2.36337192149772;0.187860853104025]
Robot Arm 2 end: [-1.82260446625651;0.402652472069155;0.907129374417922]
Robot Arm 1 togoal: 13.2461999575307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.986907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.992651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 147:
Mobile Platform 1 Pose: [-23.0984860576764;6.44016634982839;-2.38890935051227]
Robot Arm 1 end: [7.07276770686583;-2.00055364414998;0.183762629094051]
Robot Arm 1 togoal: 12.4245796890775
Mobile Platform 2 Pose: [14.8463292359633;-2.04953606228338;0.156081397569221]
Robot Arm 2 end: [-1.83195174439665;0.618749879137668;0.724726535219747]
Robot Arm 1 togoal: 13.4204661890809

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.740502e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.793421e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 148:
Mobile Platform 1 Pose: [-23.6210517354159;6.7246437316887;-2.40705039985814]
Robot Arm 1 end: [7.32835201488471;-1.89844194955766;0.39536863320243]
Robot Arm 1 togoal: 12.2465370516437
Mobile Platform 2 Pose: [14.5000879421358;-1.84343622945614;0.150724558335806]
Robot Arm 2 end: [-1.88574156252171;0.449861934882116;0.414207437193705]
Robot Arm 1 togoal: 13.2765128789667

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.039674e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.697137e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 149:
Mobile Platform 1 Pose: [-24.1242140101385;6.99981255551509;-2.42232661517207]
Robot Arm 1 end: [7.49202881560136;-1.90598501642534;0.758453682967181]
Robot Arm 1 togoal: 12.191139223171
Mobile Platform 2 Pose: [14.0877032680453;-1.59741942154491;0.0793203109089144]
Robot Arm 2 end: [-1.76198806853068;0.130866871426192;0.257251672880534]
Robot Arm 1 togoal: 13.1185503766189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.862053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.473035e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 150:
Mobile Platform 1 Pose: [-24.5861816431355;7.26362510528392;-2.4111240306217]
Robot Arm 1 end: [7.86977389905416;-2.14234566318215;0.951747938442249]
Robot Arm 1 togoal: 12.3409266003383
Mobile Platform 2 Pose: [14.4734090704012;-1.82329646138124;0.0343007275935156]
Robot Arm 2 end: [-2.12267290427464;-0.0511172429154419;0.179277560013328]
Robot Arm 1 togoal: 12.7605943981159

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.249213e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.883498e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 151:
Mobile Platform 1 Pose: [-25.0778015117757;7.54015201548496;-2.42285656444999]
Robot Arm 1 end: [8.27339782934418;-2.54670348485228;0.824019155009649]
Robot Arm 1 togoal: 12.684112333577
Mobile Platform 2 Pose: [14.0449389886447;-1.59232912898992;0.129977743475233]
Robot Arm 2 end: [-1.90453185829672;0.0523339802835209;0.251150657048019]
Robot Arm 1 togoal: 12.9691379913351

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.895864e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.176237e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 152:
Mobile Platform 1 Pose: [-25.5258119767422;7.79943735730419;-2.44256723810059]
Robot Arm 1 end: [8.40984732772279;-2.89195990926664;0.587408746318498]
Robot Arm 1 togoal: 13.0231643025894
Mobile Platform 2 Pose: [13.6078315039507;-1.33777173596629;0.160036378273071]
Robot Arm 2 end: [-1.51948386835905;0.5024745763998;0.237728640996475]
Robot Arm 1 togoal: 13.55979024424

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.829991e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.164280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 153:
Mobile Platform 1 Pose: [-25.9858666748212;8.07644805095475;-2.47716483347579]
Robot Arm 1 end: [8.38391129854427;-3.04294715352742;0.281014699651955]
Robot Arm 1 togoal: 13.2010521052589
Mobile Platform 2 Pose: [13.2982824681361;-1.16421240938231;0.147440806254318]
Robot Arm 2 end: [-1.23478838877558;0.36722499245275;0.00121794852489127]
Robot Arm 1 togoal: 13.6608208434803

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.747468e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.053022e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 154:
Mobile Platform 1 Pose: [-26.1594566009831;8.47716060962588;-2.48955889849957]
Robot Arm 1 end: [8.10308118616323;-2.04090657433313;0.228822350960455]
Robot Arm 1 togoal: 12.2577100306209
Mobile Platform 2 Pose: [12.4497622508319;-0.677215258107805;0.0533628038908854]
Robot Arm 2 end: [-0.899637703673748;0.122811820540954;0.0341981539588773]
Robot Arm 1 togoal: 13.6930089026434

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.539028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.412142e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 155:
Mobile Platform 1 Pose: [-26.3933520658926;8.5227175239938;-2.35135304424555]
Robot Arm 1 end: [8.72051849025805;-2.92314167507202;0.659503580509731]
Robot Arm 1 togoal: 13.0148699481996
Mobile Platform 2 Pose: [13.1212574479682;-1.04698269268565;-0.0856015556893591]
Robot Arm 2 end: [-0.837108587771283;-0.121123052924462;0.521289581037307]
Robot Arm 1 togoal: 13.5111143022897

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.089742e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.656662e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 156:
Mobile Platform 1 Pose: [-26.3692627426167;8.50336365645096;-2.34761040096251]
Robot Arm 1 end: [7.91972102793886;-2.7045584052097;0.848025813366251]
Robot Arm 1 togoal: 12.8913249562975
Mobile Platform 2 Pose: [12.7358318102731;-0.81482281789284;-0.178466528909173]
Robot Arm 2 end: [-0.632321977500763;-0.336274273043496;0.454243184031172]
Robot Arm 1 togoal: 13.5010724227178

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.785888e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 157:
Mobile Platform 1 Pose: [-26.4395640205634;8.51940132915427;-2.33457503476939]
Robot Arm 1 end: [7.91491770501279;-2.50396254173483;0.996333426773612]
Robot Arm 1 togoal: 12.6874710811508
Mobile Platform 2 Pose: [12.9839333400764;-1.06094303307581;-0.222339678666315]
Robot Arm 2 end: [-0.939727007226411;-0.473986435440341;0.403732433114057]
Robot Arm 1 togoal: 13.1940050004452

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.261390e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.458896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 158:
Mobile Platform 1 Pose: [-26.0029697792115;8.16229791464426;-2.4200531167784]
Robot Arm 1 end: [7.65766755217309;-2.62692807996634;1.12679858707341]
Robot Arm 1 togoal: 12.8483940158581
Mobile Platform 2 Pose: [13.116945612743;-1.25056119807046;-0.382385457728713]
Robot Arm 2 end: [-1.61708665101577;-0.576824396674733;0.0327327885357384]
Robot Arm 1 togoal: 12.699780075277

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.500695e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.862200e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 159:
Mobile Platform 1 Pose: [-25.8646876888901;8.04205940133971;-2.44131810961403]
Robot Arm 1 end: [7.63053986951284;-2.36817126088145;1.16731040612902]
Robot Arm 1 togoal: 12.5980592940847
Mobile Platform 2 Pose: [12.9473607070203;-1.16700752920515;-0.368049526956929]
Robot Arm 2 end: [-1.39425049318513;-0.963158137329603;0.893315063636511]
Robot Arm 1 togoal: 12.4946958268016

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.138190e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.219922e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 160:
Mobile Platform 1 Pose: [-25.5969345796789;7.85703466230731;-2.45210729828729]
Robot Arm 1 end: [7.48539939224566;-2.3770387015627;1.05588390736955]
Robot Arm 1 togoal: 12.638458617099
Mobile Platform 2 Pose: [12.7632863174387;-1.14145605767951;-0.326789828486472]
Robot Arm 2 end: [-0.489165333607401;-0.590165755228536;0.6351874006532]
Robot Arm 1 togoal: 13.4084160334728

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.113185e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.320103e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 161:
Mobile Platform 1 Pose: [-25.7576493793554;7.94075733782525;-2.41645259288744]
Robot Arm 1 end: [7.4363765072423;-2.5253283493829;0.972654011777805]
Robot Arm 1 togoal: 12.7967456407976
Mobile Platform 2 Pose: [12.6472328139152;-1.08150748831912;-0.344690987284201]
Robot Arm 2 end: [-0.550394218234249;-0.206403127463135;0.879365959590849]
Robot Arm 1 togoal: 13.6242901970839

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.162091e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.655696e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 162:
Mobile Platform 1 Pose: [-25.8492924573048;8.00377957325299;-2.40390970561123]
Robot Arm 1 end: [7.59386809730365;-2.51347725701449;0.66510324052075]
Robot Arm 1 togoal: 12.7714189915561
Mobile Platform 2 Pose: [12.5413031896238;-1.04467955255686;-0.376828192566767]
Robot Arm 2 end: [-0.488685802880135;-0.340063696119757;0.944584227236148]
Robot Arm 1 togoal: 13.5687774797308

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.777149e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.538505e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 163:
Mobile Platform 1 Pose: [-26.1610089288908;8.19851442561053;-2.34350412684265]
Robot Arm 1 end: [8.11532377468902;-2.26902539833247;0.618171569553586]
Robot Arm 1 togoal: 12.4457256869177
Mobile Platform 2 Pose: [12.5404453278927;-1.05155346646506;-0.382465353632579]
Robot Arm 2 end: [-0.573458521252191;-0.290164686593747;1.03541414392201]
Robot Arm 1 togoal: 13.5416535064093

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.426973e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.233461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 164:
Mobile Platform 1 Pose: [-26.0686829970918;8.13312351969942;-2.32597245804665]
Robot Arm 1 end: [8.17072914889402;-2.29211062465639;0.779649640108772]
Robot Arm 1 togoal: 12.4495709087345
Mobile Platform 2 Pose: [12.5272328385829;-1.05561024803665;-0.395504950680874]
Robot Arm 2 end: [-0.569090622825242;-0.465232175354093;1.12181551295503]
Robot Arm 1 togoal: 13.416899694232

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.541651e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.076497e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 165:
Mobile Platform 1 Pose: [-26.0488913341486;8.12025601996886;-2.31772260618092]
Robot Arm 1 end: [8.21660815388392;-2.42684645215228;0.900266611166151]
Robot Arm 1 togoal: 12.569499161151
Mobile Platform 2 Pose: [12.2965773584276;-0.951125102019304;-0.411481341131275]
Robot Arm 2 end: [-0.488338192988733;-0.928486703825083;1.01886089874508]
Robot Arm 1 togoal: 13.1535625398842

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.663165e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.703392e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 166:
Mobile Platform 1 Pose: [-26.026373937193;8.10712728726128;-2.30883120869834]
Robot Arm 1 end: [8.27783645323929;-2.70020302545211;0.852330022425347]
Robot Arm 1 togoal: 12.8338662806032
Mobile Platform 2 Pose: [11.8830843620333;-0.761441087631513;-0.478888022877936]
Robot Arm 2 end: [-0.693647706475787;-1.1193295118754;1.21088971659971]
Robot Arm 1 togoal: 12.8674266936984

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.308052e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.890961e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 167:
Mobile Platform 1 Pose: [-26.047798998152;8.1161671312615;-2.31591041685655]
Robot Arm 1 end: [8.34307842606627;-2.53027535427401;0.588089515094703]
Robot Arm 1 togoal: 12.6737331330962
Mobile Platform 2 Pose: [11.9930741158727;-0.843201563278444;-0.687584687540668]
Robot Arm 2 end: [-1.11788414441938;0.0863209202648074;0.935508141766479]
Robot Arm 1 togoal: 13.452458975763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.640263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.046138e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 168:
Mobile Platform 1 Pose: [-26.7146590065716;8.4661830374675;-2.46940580492394]
Robot Arm 1 end: [8.62998965264149;-2.58350096615908;0.532090839735351]
Robot Arm 1 togoal: 12.6964312404885
Mobile Platform 2 Pose: [12.1999416900967;-0.959485084641692;-0.698550013840578]
Robot Arm 2 end: [-1.4206430362229;0.00162664936072832;0.382445296777713]
Robot Arm 1 togoal: 13.2262696312053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.028257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.980034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 169:
Mobile Platform 1 Pose: [-27.183552802445;8.70125323856679;-2.66067486377735]
Robot Arm 1 end: [8.84270823152653;-2.7440015141972;0.567984744461004]
Robot Arm 1 togoal: 12.8318797106475
Mobile Platform 2 Pose: [12.2819953533286;-1.00764986358906;-0.692949601045874]
Robot Arm 2 end: [-1.36538362714646;-0.0904325364998549;0.188853484000305]
Robot Arm 1 togoal: 13.2110083475903

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.279700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.469717e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 170:
Mobile Platform 1 Pose: [-27.3291520554009;8.7663911942354;-2.76355573321165]
Robot Arm 1 end: [8.73317502718365;-2.75218836091444;0.585662427100083]
Robot Arm 1 togoal: 12.8490470416787
Mobile Platform 2 Pose: [12.1798450012873;-0.948411858361389;-0.69443449138302]
Robot Arm 2 end: [-1.18377210349652;-0.184201341520476;0.124047288062441]
Robot Arm 1 togoal: 13.2675263145126

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.885326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.043271e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 171:
Mobile Platform 1 Pose: [-27.2539854935933;8.71404056192153;-2.74793068963296]
Robot Arm 1 end: [8.7482185499097;-2.75410220378855;0.594506479922024]
Robot Arm 1 togoal: 12.8488314669553
Mobile Platform 2 Pose: [11.8780657618333;-0.777234699804392;-0.713939161598081]
Robot Arm 2 end: [-0.955009158530875;-0.406354610797752;0.034065195243436]
Robot Arm 1 togoal: 13.2687929477258

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.656661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.099479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 172:
Mobile Platform 1 Pose: [-27.4226037133512;8.83583285514108;-2.62908155663932]
Robot Arm 1 end: [8.9180459042677;-2.65254972508575;0.588557433747983]
Robot Arm 1 togoal: 12.7329135845021
Mobile Platform 2 Pose: [11.483562457445;-0.548204628832744;-0.760128786158112]
Robot Arm 2 end: [-0.576409239251209;-0.85754460874774;-0.0835465913711835]
Robot Arm 1 togoal: 13.2273626687631

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.699699e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.348588e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 173:
Mobile Platform 1 Pose: [-27.1722371752404;8.71133390201917;-2.3718301886725]
Robot Arm 1 end: [9.1357490110521;-2.46290074238238;0.896147283683304]
Robot Arm 1 togoal: 12.508447769546
Mobile Platform 2 Pose: [11.5034126630051;-0.578376961807048;-0.777095866532626]
Robot Arm 2 end: [-0.619958538100969;-0.749587628499016;-0.104510745836996]
Robot Arm 1 togoal: 13.2739441031893

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.895112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.593297e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 174:
Mobile Platform 1 Pose: [-26.8604637739804;8.51258153222457;-2.36755579933023]
Robot Arm 1 end: [8.85078824710596;-2.36787233174433;0.847464345350062]
Robot Arm 1 togoal: 12.4393972500842
Mobile Platform 2 Pose: [11.4157599499111;-0.518006361853551;-0.802311669484956]
Robot Arm 2 end: [-0.518742256592054;-0.619520774143957;-0.0678322333575731]
Robot Arm 1 togoal: 13.4317544188913

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.397174e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.753758e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 175:
Mobile Platform 1 Pose: [-26.875787351193;8.52236064237734;-2.36754756646754]
Robot Arm 1 end: [8.71231976088376;-2.04490130151644;0.834651709236286]
Robot Arm 1 togoal: 12.132965084336
Mobile Platform 2 Pose: [11.0379871572873;-0.288132657239387;-0.842261617961049]
Robot Arm 2 end: [-0.270553432519147;-0.505891071607751;0.00498717111746755]
Robot Arm 1 togoal: 13.6783964612442

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.479706e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.422054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 176:
Mobile Platform 1 Pose: [-26.4825299516423;8.28529015505353;-2.36498605094494]
Robot Arm 1 end: [8.55630497888915;-2.04905471522603;0.819735779007114]
Robot Arm 1 togoal: 12.155482152187
Mobile Platform 2 Pose: [10.04130544447;0.26475707591736;-0.883621618895887]
Robot Arm 2 end: [0.287243957385453;-0.446974053884843;0.0692782088222176]
Robot Arm 1 togoal: 14.1136525791481

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.258181e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.827737e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 177:
Mobile Platform 1 Pose: [-26.7826187535932;8.46637222129083;-2.35477256278387]
Robot Arm 1 end: [9.02264134341461;-2.34915463640349;0.890096615361686]
Robot Arm 1 togoal: 12.4038255896379
Mobile Platform 2 Pose: [8.95417115455254;0.835737324572624;-0.911034722537283]
Robot Arm 2 end: [0.825575266067803;-0.567713117839771;0.00107903295067696]
Robot Arm 1 togoal: 14.4385343928928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.887699e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.702383e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 178:
Mobile Platform 1 Pose: [-27.2252885492304;8.71993145806737;-2.38407919110249]
Robot Arm 1 end: [9.28807416684562;-2.58113521151337;0.918905139790166]
Robot Arm 1 togoal: 12.6156379078898
Mobile Platform 2 Pose: [8.27669810005791;1.23168280072908;-1.00681204689346]
Robot Arm 2 end: [1.32671990961762;-1.58917964560114;0.198221603497809]
Robot Arm 1 togoal: 14.1699056324011

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.966189e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 179:
Mobile Platform 1 Pose: [-28.050113983286;9.20426778349731;-2.42343004738515]
Robot Arm 1 end: [9.70552534588405;-2.66412655814903;0.887274820078297]
Robot Arm 1 togoal: 12.6833916759832
Mobile Platform 2 Pose: [8.29385776509114;1.23756541847963;-0.983019657664186]
Robot Arm 2 end: [1.31587076327082;-1.23533707084535;-0.145073421280411]
Robot Arm 1 togoal: 14.4098594038194

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.695081e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.781195e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 180:
Mobile Platform 1 Pose: [-29.0152348164593;9.69813877488215;-2.48744718802643]
Robot Arm 1 end: [9.92640671620128;-2.36041093917057;1.08524285547259]
Robot Arm 1 togoal: 12.3683086493503
Mobile Platform 2 Pose: [8.41532274123984;1.1385185010693;-0.93985284903557]
Robot Arm 2 end: [1.17091807944444;-1.02444964440843;-0.195841453287002]
Robot Arm 1 togoal: 14.4325139701697

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.633277e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.651218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 181:
Mobile Platform 1 Pose: [-28.094110511236;9.18916388756741;-2.41023108895875]
Robot Arm 1 end: [9.44736639717604;-2.21962355261102;1.07621960290875]
Robot Arm 1 togoal: 12.2401974358314
Mobile Platform 2 Pose: [8.46623877179825;1.10510493312916;-0.873348372288626]
Robot Arm 2 end: [1.15099936556877;-0.791532722179595;-0.176841893128527]
Robot Arm 1 togoal: 14.5610206905858

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.903597e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.308053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 182:
Mobile Platform 1 Pose: [-27.1117221325424;8.64146147475653;-2.35139421197357]
Robot Arm 1 end: [8.81209717549589;-1.96186854829516;1.01175240143978]
Robot Arm 1 togoal: 12.0314897416653
Mobile Platform 2 Pose: [8.44566811294861;1.12139173322368;-0.845589054400102]
Robot Arm 2 end: [1.23561760394003;-0.629794732019098;-0.11759885080807]
Robot Arm 1 togoal: 14.7215779011604

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.217784e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.058972e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 183:
Mobile Platform 1 Pose: [-27.1291404594159;8.6515666649114;-2.3405855129013]
Robot Arm 1 end: [9.03103228401412;-2.17882132622138;1.01366046672911]
Robot Arm 1 togoal: 12.2278363062591
Mobile Platform 2 Pose: [8.38903518287503;1.15655429762628;-0.84785223350453]
Robot Arm 2 end: [1.48488546901769;-0.635242089375407;0.0316367484809875]
Robot Arm 1 togoal: 14.8936056029437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.182965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.329999e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 184:
Mobile Platform 1 Pose: [-27.4103266642108;8.83435348469253;-2.30586111518568]
Robot Arm 1 end: [9.42020559035672;-2.19512374083032;0.508921426030826]
Robot Arm 1 togoal: 12.2507757979446
Mobile Platform 2 Pose: [9.2847657182674;0.615888738205055;-0.900862680038409]
Robot Arm 2 end: [0.641960057756273;-0.858876919554764;1.3589253922516]
Robot Arm 1 togoal: 14.0298864293766

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.992067e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.515731e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 185:
Mobile Platform 1 Pose: [-27.2567549808865;8.78523939677106;-2.22522304631594]
Robot Arm 1 end: [8.97178537561578;-1.59045224262957;0.849778181636708]
Robot Arm 1 togoal: 11.6553141198305
Mobile Platform 2 Pose: [9.08272415277527;0.744044165983206;-1.01181017569553]
Robot Arm 2 end: [0.712879332422964;-0.960913367591952;1.3720243962772]
Robot Arm 1 togoal: 14.0175983845214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.107740e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 186:
Mobile Platform 1 Pose: [-27.5857887360405;8.97545527108741;-2.22469183255886]
Robot Arm 1 end: [9.02883229223811;-1.73270294364879;0.21294242861802]
Robot Arm 1 togoal: 11.8452733058615
Mobile Platform 2 Pose: [9.32822032331277;0.581729139996162;-1.01743683818532]
Robot Arm 2 end: [-0.104071120239225;-0.919051774535268;0.545353076258603]
Robot Arm 1 togoal: 13.4664366477053

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.645211e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.346740e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 187:
Mobile Platform 1 Pose: [-27.9661860559433;9.19801903457999;-2.207296857088]
Robot Arm 1 end: [8.97292564124715;-1.67002860212856;0.346978167738987]
Robot Arm 1 togoal: 11.7738174172836
Mobile Platform 2 Pose: [8.9275712577356;0.835516547506972;-1.02319199094825]
Robot Arm 2 end: [0.385939649921502;-0.629769877931285;0.290268778215282]
Robot Arm 1 togoal: 14.0422097368118

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.291207e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.407898e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 188:
Mobile Platform 1 Pose: [-28.0134744649261;9.21739884653959;-2.1896724798017]
Robot Arm 1 end: [8.92756221134312;-1.67147016255401;0.442656949157817]
Robot Arm 1 togoal: 11.7701385846228
Mobile Platform 2 Pose: [8.61021881360507;1.01209345566158;-0.993419181714218]
Robot Arm 2 end: [0.856884025994635;-0.369564295017861;0.272332700442797]
Robot Arm 1 togoal: 14.5662758674507

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.862112e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.890371e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 189:
Mobile Platform 1 Pose: [-27.9970905726772;9.18375946870162;-2.14833003039156]
Robot Arm 1 end: [8.6187890329491;-2.29754478600995;0.535619095358858]
Robot Arm 1 togoal: 12.414037499003
Mobile Platform 2 Pose: [8.14905125839607;1.26699513344504;-0.855359220567429]
Robot Arm 2 end: [1.01590356639813;-0.212030716242076;0.753711385891537]
Robot Arm 1 togoal: 14.7561243523261

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.877412e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.510927e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 190:
Mobile Platform 1 Pose: [-27.3213742223462;8.78869333391143;-2.27204022461535]
Robot Arm 1 end: [8.10354411969542;-2.28396143774708;0.826380651783332]
Robot Arm 1 togoal: 12.4488854741666
Mobile Platform 2 Pose: [8.10564105487334;1.29439591713316;-0.872966917930006]
Robot Arm 2 end: [1.6077729294438;-0.18463742203831;0.791468601911938]
Robot Arm 1 togoal: 15.2188682555299

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.186386e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.067256e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 191:
Mobile Platform 1 Pose: [-27.1287353154394;8.67854493062618;-2.29668751262665]
Robot Arm 1 end: [8.23352366444306;-1.87099668399042;0.551393809079497]
Robot Arm 1 togoal: 12.0408113132324
Mobile Platform 2 Pose: [8.01419147567177;1.34828686830787;-0.870372649101463]
Robot Arm 2 end: [1.19344493398252;-0.255042130259418;0.344712240519393]
Robot Arm 1 togoal: 14.887614525713

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.626658e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.883536e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 192:
Mobile Platform 1 Pose: [-27.077771550785;8.63858853585351;-2.28772074602304]
Robot Arm 1 end: [8.39845417154688;-1.72377436166759;0.529330656341016]
Robot Arm 1 togoal: 11.8741417547157
Mobile Platform 2 Pose: [7.85059714255074;1.44342477552712;-0.875757062587599]
Robot Arm 2 end: [1.38969408180844;-0.342152803960082;0.317654173242001]
Robot Arm 1 togoal: 14.9815614979886

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.018256e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.882507e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 193:
Mobile Platform 1 Pose: [-26.6846405465729;8.3954115680847;-2.2746600167009]
Robot Arm 1 end: [8.03268583647351;-1.74191081467476;0.691011623449363]
Robot Arm 1 togoal: 11.9344742365551
Mobile Platform 2 Pose: [7.09254583292389;1.86844432101947;-0.914490630919293]
Robot Arm 2 end: [2.01032213504459;-0.459479393474813;0.282598141855489]
Robot Arm 1 togoal: 15.3884050633368

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.670421e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.821517e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 194:
Mobile Platform 1 Pose: [-26.6528760121681;8.37990950059002;-2.27886451890533]
Robot Arm 1 end: [7.96429381651864;-1.78189884317162;0.661972528420426]
Robot Arm 1 togoal: 11.9872919466042
Mobile Platform 2 Pose: [7.55120704186563;1.57385443777599;-1.08125159672541]
Robot Arm 2 end: [1.90632157337081;-1.19804379442987;0.867116130599243]
Robot Arm 1 togoal: 14.8210151665786

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.606494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.155527e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 195:
Mobile Platform 1 Pose: [-26.5735042157382;8.33273289582693;-2.29864090793235]
Robot Arm 1 end: [7.8959490576451;-1.76344368086104;0.714976005490136]
Robot Arm 1 togoal: 11.9772831761022
Mobile Platform 2 Pose: [7.97629562403122;1.28638464000695;-1.03385544494328]
Robot Arm 2 end: [1.48433239522817;-0.778413509108437;0.814691720985559]
Robot Arm 1 togoal: 14.7453863753408

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.424486e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.615485e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 196:
Mobile Platform 1 Pose: [-26.5761143578871;8.32462672260977;-2.28672599868494]
Robot Arm 1 end: [7.80428489555749;-1.80556713746237;0.946369323888308]
Robot Arm 1 togoal: 12.0217627938226
Mobile Platform 2 Pose: [8.38353927068607;1.03646246492606;-1.04010243760854]
Robot Arm 2 end: [1.27839090118621;-0.815573097628202;0.906163777201124]
Robot Arm 1 togoal: 14.5579470539577

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.388575e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.431959e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 197:
Mobile Platform 1 Pose: [-26.5144991795604;8.27904941322298;-2.27982419702525]
Robot Arm 1 end: [7.76711978890847;-1.8957689070964;1.16096833987731]
Robot Arm 1 togoal: 12.1088684328643
Mobile Platform 2 Pose: [8.78378688067385;0.786469928322324;-1.03817029231582]
Robot Arm 2 end: [0.922034714233203;-0.695505433482803;0.947846200943026]
Robot Arm 1 togoal: 14.3594208349305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.043344e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.704190e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 198:
Mobile Platform 1 Pose: [-26.3892858862712;8.19345611935826;-2.2752789845432]
Robot Arm 1 end: [7.70254054135069;-2.01581845081063;1.29385287881574]
Robot Arm 1 togoal: 12.2355959732921
Mobile Platform 2 Pose: [9.08649347689933;0.597546769085117;-1.0568843443307]
Robot Arm 2 end: [0.561610450408045;-0.602484599052956;0.912038249864816]
Robot Arm 1 togoal: 14.1503266898801

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.081768e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.418154e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 199:
Mobile Platform 1 Pose: [-26.2166590630581;8.07440471994857;-2.27293995244884]
Robot Arm 1 end: [7.68414837841736;-2.1507513838774;1.41832943841787]
Robot Arm 1 togoal: 12.3699017445654
Mobile Platform 2 Pose: [9.52863341529135;0.348570523280348;-1.0795511825038]
Robot Arm 2 end: [0.339013993344961;-0.559334384605158;0.890215728111832]
Robot Arm 1 togoal: 14.0149658013833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.599111e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.830981e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 200:
Mobile Platform 1 Pose: [-25.9986066565331;7.9247905496573;-2.27758985563023]
Robot Arm 1 end: [7.78587102263303;-2.26942608699117;1.49473263537625]
Robot Arm 1 togoal: 12.4676330394746
Mobile Platform 2 Pose: [9.80579232075631;0.183624588712531;-1.10312193276885]
Robot Arm 2 end: [0.319272332506716;-0.616049800447768;0.867876652631892]
Robot Arm 1 togoal: 13.9632543816616

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.005516e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.562674e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 201:
Mobile Platform 1 Pose: [-26.3030118257823;8.10836814586986;-2.17334413824247]
Robot Arm 1 end: [7.63010149245126;-2.16744810308711;1.32957202984123]
Robot Arm 1 togoal: 12.3975746396285
Mobile Platform 2 Pose: [9.64536950299205;0.281784216590642;-1.09049242366729]
Robot Arm 2 end: [0.39123127961285;-0.688873896575938;0.679011240570289]
Robot Arm 1 togoal: 13.9779720235546

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.283626e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.063303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 202:
Mobile Platform 1 Pose: [-26.4195048289795;8.20070423586657;-2.1268865919923]
Robot Arm 1 end: [7.57555434518426;-2.56510229826027;0.979346125905991]
Robot Arm 1 togoal: 12.8083223498716
Mobile Platform 2 Pose: [9.78639676121486;0.189839968329461;-1.12539795324172]
Robot Arm 2 end: [0.190425082358598;-1.04900003423785;0.677906642585732]
Robot Arm 1 togoal: 13.5895537143629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.841693e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.674968e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 203:
Mobile Platform 1 Pose: [-26.2003894992987;8.1142514589185;-2.11396049981844]
Robot Arm 1 end: [7.99423364369078;-2.96120283434953;0.462132462981902]
Robot Arm 1 togoal: 13.1581563317284
Mobile Platform 2 Pose: [9.78333576037798;0.190498458256846;-1.18269314209633]
Robot Arm 2 end: [0.15780139630885;-1.00983640105011;0.530546763089306]
Robot Arm 1 togoal: 13.6009179233331

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.029096e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 204:
Mobile Platform 1 Pose: [-26.1576641929708;8.10352681851111;-2.11647026605098]
Robot Arm 1 end: [8.14416502660599;-2.95706180011831;0.381015804715061]
Robot Arm 1 togoal: 13.1388408128745
Mobile Platform 2 Pose: [9.54513957311415;0.341398325138072;-1.20274321916264]
Robot Arm 2 end: [-0.0661723820620059;-0.836196337553999;0.538947207598828]
Robot Arm 1 togoal: 13.5506699610694

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.469317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.612764e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 205:
Mobile Platform 1 Pose: [-26.3325439638151;8.16994003155802;-2.15712393457427]
Robot Arm 1 end: [8.07190649874494;-2.96452606115346;0.31101284193391]
Robot Arm 1 togoal: 13.1628473159341
Mobile Platform 2 Pose: [9.60821394068232;0.305261659550535;-1.2068573304713]
Robot Arm 2 end: [-0.12485449442212;-0.853925287238492;0.518680614892951]
Robot Arm 1 togoal: 13.4971784294908

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.155493e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.922748e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 206:
Mobile Platform 1 Pose: [-26.5433620621074;8.22481380868284;-2.2011000858874]
Robot Arm 1 end: [7.93012747507113;-2.72659022676113;0.428195639869708]
Robot Arm 1 togoal: 12.9400422116339
Mobile Platform 2 Pose: [9.55259861357634;0.325177291147442;-1.17327271636553]
Robot Arm 2 end: [-0.0237611605768699;-0.72567331465563;0.495932238224602]
Robot Arm 1 togoal: 13.6597672294932

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.310814e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.295880e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 207:
Mobile Platform 1 Pose: [-26.6771979621127;8.15754338721401;-2.19340352251974]
Robot Arm 1 end: [7.98175523793278;-2.72706838656911;0.359047922928317]
Robot Arm 1 togoal: 12.9383814391689
Mobile Platform 2 Pose: [9.11968586332177;0.563819152153642;-1.11103779748296]
Robot Arm 2 end: [0.272464107831499;-0.576626729770769;0.66401017537658]
Robot Arm 1 togoal: 13.9663135506928

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.992040e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.265519e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 208:
Mobile Platform 1 Pose: [-26.4917586804774;8.060632986287;-2.11136439599087]
Robot Arm 1 end: [7.74465392500084;-1.88781809200846;0.378288362938479]
Robot Arm 1 togoal: 12.1537070390961
Mobile Platform 2 Pose: [8.84660314420665;0.727555071875812;-1.13434823887557]
Robot Arm 2 end: [0.602506013966492;-0.554332983542211;0.811095411447635]
Robot Arm 1 togoal: 14.2175146803807

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.645191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.793713e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 209:
Mobile Platform 1 Pose: [-26.2311287303588;7.8915066091698;-2.09940484238265]
Robot Arm 1 end: [7.85219477405166;-1.83321184137638;0.571213269820203]
Robot Arm 1 togoal: 12.0639986987867
Mobile Platform 2 Pose: [8.54905155034153;0.901174304760661;-1.1447668396881]
Robot Arm 2 end: [0.93204874560294;-0.601790103016551;0.83612852407805]
Robot Arm 1 togoal: 14.432778248921

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.876880e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.241090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 210:
Mobile Platform 1 Pose: [-26.3904917785013;7.96849713087155;-1.96593082355237]
Robot Arm 1 end: [7.91486527325956;-1.73866471090033;0.560741298805504]
Robot Arm 1 togoal: 11.9610255746243
Mobile Platform 2 Pose: [8.63550198239463;0.843995274372743;-1.20431601748674]
Robot Arm 2 end: [0.876562451549717;-0.592910025436912;0.591183969400731]
Robot Arm 1 togoal: 14.4103265196437

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.284399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.384643e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 211:
Mobile Platform 1 Pose: [-26.1330889738098;7.77272847106178;-1.81044888934092]
Robot Arm 1 end: [7.64078284072222;-2.00310354050453;0.796788476036103]
Robot Arm 1 togoal: 12.2541781666559
Mobile Platform 2 Pose: [8.43893704594576;0.962049382615533;-1.17165655027783]
Robot Arm 2 end: [0.94117424079726;-0.589214020624812;0.709567153926049]
Robot Arm 1 togoal: 14.4544321846546

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.344031e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.597072e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 212:
Mobile Platform 1 Pose: [-25.9269157871718;7.60093412185622;-1.79074454036644]
Robot Arm 1 end: [7.73254214398151;-1.78099367169319;0.660358858164747]
Robot Arm 1 togoal: 12.0280455684336
Mobile Platform 2 Pose: [8.62092650006143;0.854564539804748;-1.19334010483535]
Robot Arm 2 end: [0.939924445800697;-0.665892126268775;0.448628988294738]
Robot Arm 1 togoal: 14.4207314746869

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.533013e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.532837e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 213:
Mobile Platform 1 Pose: [-25.9793720838987;7.63955148244874;-1.77315710556269]
Robot Arm 1 end: [7.7799217637654;-1.69734078564964;0.709795344176362]
Robot Arm 1 togoal: 11.9337580763251
Mobile Platform 2 Pose: [8.33736883243504;1.01741012402122;-1.21948552126756]
Robot Arm 2 end: [0.861944257598164;-1.16342582984823;0.367651796876412]
Robot Arm 1 togoal: 14.0498074078406

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.151760e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.418859e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 214:
Mobile Platform 1 Pose: [-26.1162978756009;7.73199498218006;-1.75226169075391]
Robot Arm 1 end: [7.89159362769434;-1.59493367998219;0.744423997295709]
Robot Arm 1 togoal: 11.8106280426547
Mobile Platform 2 Pose: [8.72404857111237;0.767627506676417;-1.22990876871969]
Robot Arm 2 end: [0.511575250635118;-1.18199208804261;0.412966096773211]
Robot Arm 1 togoal: 13.7651086779512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.136896e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006166e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 215:
Mobile Platform 1 Pose: [-26.3523147743924;7.88401547534175;-1.74685659573923]
Robot Arm 1 end: [8.09841731023697;-1.56427666985018;0.800816507959711]
Robot Arm 1 togoal: 11.7416853937182
Mobile Platform 2 Pose: [8.75319998579813;0.748548984803537;-1.23135882544732]
Robot Arm 2 end: [0.514912031902797;-1.04209704238044;0.412798653520732]
Robot Arm 1 togoal: 13.8576877819181

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.720192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 216:
Mobile Platform 1 Pose: [-26.3773037378375;7.90813369338357;-1.76029164531709]
Robot Arm 1 end: [8.15384979813458;-1.52126116408068;0.728617905783242]
Robot Arm 1 togoal: 11.6951100363373
Mobile Platform 2 Pose: [8.66351933734528;0.80789263834378;-1.24335582530128]
Robot Arm 2 end: [0.652131734710383;-0.915120325440997;0.444790858575973]
Robot Arm 1 togoal: 14.041409306438

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.934826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 217:
Mobile Platform 1 Pose: [-25.7485967320571;7.55612254266285;-1.64392481673461]
Robot Arm 1 end: [7.41063185122229;-1.1156553305877;1.01220699791552]
Robot Arm 1 togoal: 11.424600262473
Mobile Platform 2 Pose: [8.57638024630539;0.861027359418245;-1.24730606043167]
Robot Arm 2 end: [0.80902133200845;-0.822295579851496;0.510038932503585]
Robot Arm 1 togoal: 14.2157392653151

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.365046e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.349721e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 218:
Mobile Platform 1 Pose: [-25.8757365011286;7.6068014667468;-1.58054715636455]
Robot Arm 1 end: [7.61230801296761;-1.01034913997493;0.947087141425758]
Robot Arm 1 togoal: 11.2808792732802
Mobile Platform 2 Pose: [8.74896026259956;0.758430499739371;-1.21347332556237]
Robot Arm 2 end: [0.600555284206343;-0.87589834762417;0.438100964412339]
Robot Arm 1 togoal: 14.0283168490621

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.343986e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 219:
Mobile Platform 1 Pose: [-26.3129388072303;7.84606014852005;-1.4763274969258]
Robot Arm 1 end: [8.33783497728542;-1.54711680945247;0.312424765078182]
Robot Arm 1 togoal: 11.7285699585389
Mobile Platform 2 Pose: [8.96297433322785;0.627613702220214;-1.20242552343232]
Robot Arm 2 end: [0.599920588387195;-0.975158964782241;0.382269797920384]
Robot Arm 1 togoal: 13.9679196254916

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.335008e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.124026e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 220:
Mobile Platform 1 Pose: [-26.2381974913409;7.83610935691835;-1.43602497154693]
Robot Arm 1 end: [8.40000949632401;-0.816188999544366;0.607203807561326]
Robot Arm 1 togoal: 10.9720069981309
Mobile Platform 2 Pose: [9.07552099886222;0.557461780178028;-1.21344750123286]
Robot Arm 2 end: [0.540195957590061;-0.893137488779828;0.415108126465955]
Robot Arm 1 togoal: 13.9733200227653

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.028661e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.259759e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 221:
Mobile Platform 1 Pose: [-26.5298002893879;8.0162507138373;-1.39863492747369]
Robot Arm 1 end: [8.65537777558309;-1.24505214996098;1.15050877432475]
Robot Arm 1 togoal: 11.3312166394452
Mobile Platform 2 Pose: [9.17794089043021;0.495823681883578;-1.19321010978057]
Robot Arm 2 end: [0.480858557296067;-0.776987314221753;0.478771738913403]
Robot Arm 1 togoal: 13.9999499587777

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.375595e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 222:
Mobile Platform 1 Pose: [-26.8631353759033;8.33111837919917;-1.19882518373016]
Robot Arm 1 end: [8.49254687127988;-0.870903463036707;-0.152558648116067]
Robot Arm 1 togoal: 11.1017906477575
Mobile Platform 2 Pose: [8.97016771301582;0.62205942047374;-1.16005135336344]
Robot Arm 2 end: [0.633523769287413;-0.7018022260219;0.557640277692218]
Robot Arm 1 togoal: 14.1582616005842

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.566302e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.548865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 223:
Mobile Platform 1 Pose: [-27.0365963298052;8.43054076031688;-1.18518348886246]
Robot Arm 1 end: [8.78627494269723;-0.466440390307373;0.335412868286075]
Robot Arm 1 togoal: 10.603071244006
Mobile Platform 2 Pose: [8.71998279029936;0.776628309102929;-1.09347008674236]
Robot Arm 2 end: [0.803938756026187;-0.603511563337591;0.736571065365605]
Robot Arm 1 togoal: 14.3399587285943

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.247970e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 224:
Mobile Platform 1 Pose: [-27.2637955166241;8.5583902371197;-1.1811867637929]
Robot Arm 1 end: [9.27161858744412;-0.793719872226613;0.495791241339242]
Robot Arm 1 togoal: 10.8667373742578
Mobile Platform 2 Pose: [9.11064773181082;0.538272433619344;-1.16268872734848]
Robot Arm 2 end: [0.454027823190703;-0.621483102442359;0.728757298110767]
Robot Arm 1 togoal: 14.0666600664307

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.461079e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.698261e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 225:
Mobile Platform 1 Pose: [-27.6016057248772;8.76258510852677;-1.16081027396639]
Robot Arm 1 end: [9.55968882421681;-1.28826084858316;0.728242188496818]
Robot Arm 1 togoal: 11.3246267870016
Mobile Platform 2 Pose: [9.46348542756232;0.331371185267741;-1.2349174435959]
Robot Arm 2 end: [0.514532771326544;-0.729064043407427;0.769416108496169]
Robot Arm 1 togoal: 14.0381811947456

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.566507e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.448251e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 226:
Mobile Platform 1 Pose: [-27.6453622622259;8.86119040395509;-1.08831606711232]
Robot Arm 1 end: [9.12915467305745;-0.339701282147621;0.712118213572008]
Robot Arm 1 togoal: 10.4077895795369
Mobile Platform 2 Pose: [9.07743662851418;0.55132027133386;-1.26773383726541]
Robot Arm 2 end: [0.574709324229233;-0.830170927899844;0.600702738685187]
Robot Arm 1 togoal: 14.0270164166389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.689365e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.645406e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 227:
Mobile Platform 1 Pose: [-27.8159560872457;8.95961035414171;-1.07058388701688]
Robot Arm 1 end: [9.72769001460443;-0.804631165879766;0.634842976848173]
Robot Arm 1 togoal: 10.8443294688297
Mobile Platform 2 Pose: [9.01481031836876;0.582990417379607;-1.32518782186279]
Robot Arm 2 end: [0.593965498056318;-0.872893125609866;0.542342724053281]
Robot Arm 1 togoal: 14.0176301468654

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.613445e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.287653e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 228:
Mobile Platform 1 Pose: [-28.3057372392096;9.26631089434706;-1.02321085187106]
Robot Arm 1 end: [9.84229701810229;-1.57647773369752;0.75274948868354]
Robot Arm 1 togoal: 11.6030132206006
Mobile Platform 2 Pose: [9.16504370019143;0.490760344515333;-1.35763130772639]
Robot Arm 2 end: [0.501478071318684;-0.996961527174551;0.466489681070396]
Robot Arm 1 togoal: 13.8725533135862

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976862e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.123065e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 229:
Mobile Platform 1 Pose: [-28.3157139303019;9.27612268484202;-1.02261444694456]
Robot Arm 1 end: [10.0093367147308;-1.42391903777132;0.612871512717613]
Robot Arm 1 togoal: 11.4599611999871
Mobile Platform 2 Pose: [9.16145894483989;0.490482678010363;-1.38837140930019]
Robot Arm 2 end: [0.558151761920298;-1.12459171658773;0.434206093798729]
Robot Arm 1 togoal: 13.8357710959512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998131e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.004451e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 230:
Mobile Platform 1 Pose: [-28.1787889295925;9.21985246398987;-1.00064928707609]
Robot Arm 1 end: [10.0175938899065;-1.20884780673541;0.461741285054758]
Robot Arm 1 togoal: 11.2588013449348
Mobile Platform 2 Pose: [9.00118546319385;0.585501950713039;-1.41840760092295]
Robot Arm 2 end: [0.704420419664721;-1.25472983692336;0.458687192255568]
Robot Arm 1 togoal: 13.863364499559

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.150841e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 231:
Mobile Platform 1 Pose: [-27.9663550375783;9.1297551406791;-0.968618756310458]
Robot Arm 1 end: [9.8262157596786;-0.575045377748678;0.735049006420739]
Robot Arm 1 togoal: 10.6056355145713
Mobile Platform 2 Pose: [8.96033933188752;0.605350021283622;-1.44668835268028]
Robot Arm 2 end: [0.61965057254918;-1.38773899778291;0.620896744609831]
Robot Arm 1 togoal: 13.7024889609048

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.380111e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 232:
Mobile Platform 1 Pose: [-28.7661229029956;9.69454015317604;-0.651873119220986]
Robot Arm 1 end: [10.0026273620688;-1.26487652022494;0.474257824539159]
Robot Arm 1 togoal: 11.3134043992441
Mobile Platform 2 Pose: [8.93200404319295;0.623553359624935;-1.45983091523234]
Robot Arm 2 end: [0.652449406718612;-1.35250888361712;0.512078000962272]
Robot Arm 1 togoal: 13.757605335623

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.279700e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 233:
Mobile Platform 1 Pose: [-28.8322633901312;9.7328609688207;-0.661442048370523]
Robot Arm 1 end: [10.1331573109333;-1.10954357659525;0.131423353755877]
Robot Arm 1 togoal: 11.1969019199737
Mobile Platform 2 Pose: [8.91129637055605;0.635385638916269;-1.4813833352088]
Robot Arm 2 end: [0.523601312515631;-1.02853736224153;0.393684387815541]
Robot Arm 1 togoal: 13.8745870906111

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.011307e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.287325e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 234:
Mobile Platform 1 Pose: [-28.8492171590899;9.74704184213073;-0.661454076206262]
Robot Arm 1 end: [10.1886323495754;-1.00622925542073;0.0605761885735787]
Robot Arm 1 togoal: 11.1043010718895
Mobile Platform 2 Pose: [8.50436610698762;0.858769629743265;-1.36888639892834]
Robot Arm 2 end: [0.886330381021938;-0.770261991105579;0.547283497437131]
Robot Arm 1 togoal: 14.3055365752199

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.829826e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.491110e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 235:
Mobile Platform 1 Pose: [-28.8511195867942;9.75322758268857;-0.657873680569161]
Robot Arm 1 end: [10.2029430884326;-0.878637222779798;-0.031965456585632]
Robot Arm 1 togoal: 10.9907977613702
Mobile Platform 2 Pose: [8.55627197392583;0.84898687039598;-1.17659804381817]
Robot Arm 2 end: [0.746986678781982;-0.347051146979446;0.508113874587198]
Robot Arm 1 togoal: 14.4811284966332

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.013326e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.526055e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 236:
Mobile Platform 1 Pose: [-28.7878503313927;9.72512656118514;-0.642827761213813]
Robot Arm 1 end: [10.3152754735661;-0.694587021348383;0.0750214010122904]
Robot Arm 1 togoal: 10.7965014838213
Mobile Platform 2 Pose: [8.45785287151412;0.90526198140359;-1.1509607552502]
Robot Arm 2 end: [0.887239494193206;-0.319752215154757;0.396581948960687]
Robot Arm 1 togoal: 14.6117588583108

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.366913e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.523255e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 237:
Mobile Platform 1 Pose: [-28.8415928494516;9.76403349952558;-0.635343342979012]
Robot Arm 1 end: [10.4750536330646;-0.494975675305119;0.203253701904499]
Robot Arm 1 togoal: 10.5880425804621
Mobile Platform 2 Pose: [8.17670357588597;1.06636293973273;-1.13286029553456]
Robot Arm 2 end: [1.10536118310191;-0.341315897272836;0.273966023146602]
Robot Arm 1 togoal: 14.7707250784397

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.394029e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.533805e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 238:
Mobile Platform 1 Pose: [-28.85349990861;9.78789519058359;-0.606114300676853]
Robot Arm 1 end: [10.5464758679172;-0.372765434000142;0.651239627670576]
Robot Arm 1 togoal: 10.4235014140297
Mobile Platform 2 Pose: [7.67504945886409;1.34934683518094;-1.10508465006199]
Robot Arm 2 end: [1.11639218987549;-0.486294172119023;0.163957692433425]
Robot Arm 1 togoal: 14.6944322014533

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.795823e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.160471e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 239:
Mobile Platform 1 Pose: [-29.5601922254951;10.2546465090619;-0.434359089416372]
Robot Arm 1 end: [11.0336672129357;-0.597736281220624;0.538297093823198]
Robot Arm 1 togoal: 10.6932776638039
Mobile Platform 2 Pose: [7.94601463885513;1.17963125567588;-1.00531786143369]
Robot Arm 2 end: [0.772730629764958;-0.351580448351702;0.37997654400741]
Robot Arm 1 togoal: 14.5067453135854

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.664049e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.113907e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 240:
Mobile Platform 1 Pose: [-29.7464306378814;10.4074511479788;-0.398082284628238]
Robot Arm 1 end: [11.2018853050846;-0.466732420362082;0.405016618872502]
Robot Arm 1 togoal: 10.5944530181123
Mobile Platform 2 Pose: [7.87388967017823;1.21671224516545;-0.988475557369922]
Robot Arm 2 end: [0.68802656644667;-0.420031084414148;0.633225570726142]
Robot Arm 1 togoal: 14.3804679946771

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.981457e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.816665e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 241:
Mobile Platform 1 Pose: [-29.9329768241275;10.5348755884193;-0.381898289789866]
Robot Arm 1 end: [11.313840404064;-0.288287082818897;0.827429459962153]
Robot Arm 1 togoal: 10.395002058675
Mobile Platform 2 Pose: [7.93693534956553;1.18705631742646;-1.00595765173866]
Robot Arm 2 end: [0.791130292042599;-0.403047827192421;0.868786415330194]
Robot Arm 1 togoal: 14.4559803039096

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.986617e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.779452e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 242:
Mobile Platform 1 Pose: [-30.3765983984575;10.8839860972727;-0.240748516673831]
Robot Arm 1 end: [11.6912679562036;-0.0435736760458821;0.698753318116139]
Robot Arm 1 togoal: 10.2181137736723
Mobile Platform 2 Pose: [7.93093530499593;1.19235823788195;-1.01279306430232]
Robot Arm 2 end: [0.73078571101421;-0.548193852258925;0.929710019001393]
Robot Arm 1 togoal: 14.3120936718501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.239280e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.690107e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 243:
Mobile Platform 1 Pose: [-30.6961047282134;11.1040169326254;-0.191030513210127]
Robot Arm 1 end: [11.8783385395469;-0.278196167413159;0.889926327891947]
Robot Arm 1 togoal: 10.4674603464547
Mobile Platform 2 Pose: [8.03651902742635;1.14162072063168;-1.04232056743455]
Robot Arm 2 end: [0.757332876412249;-0.621716983541886;1.107826822023]
Robot Arm 1 togoal: 14.2773637281441

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.518396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.483865e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 244:
Mobile Platform 1 Pose: [-30.7135152666126;11.1417020654275;-0.160854287923646]
Robot Arm 1 end: [11.7915208682185;0.294971445872477;0.713971324378838]
Robot Arm 1 togoal: 9.90194029172907
Mobile Platform 2 Pose: [8.08138339359359;1.12001408780685;-1.05736117946893]
Robot Arm 2 end: [0.677940955297016;-0.692216395977536;1.12897332537133]
Robot Arm 1 togoal: 14.1706366680449

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.873500e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.519429e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 245:
Mobile Platform 1 Pose: [-31.1174982197294;11.4053967126997;-0.0759850696938648]
Robot Arm 1 end: [12.1614465054869;-0.325235490646798;0.464568056358718]
Robot Arm 1 togoal: 10.6018105710741
Mobile Platform 2 Pose: [8.31604690708542;0.990358725735957;-1.0827354540385]
Robot Arm 2 end: [0.914162401986604;-1.7060616697592;1.09244353750941]
Robot Arm 1 togoal: 13.7146642177021

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.939525e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.259324e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 246:
Mobile Platform 1 Pose: [-31.0527385585606;11.3813061995102;-0.0612102304277235]
Robot Arm 1 end: [12.1832133466533;-0.0302665861211668;0.488934207140428]
Robot Arm 1 togoal: 10.3168710424457
Mobile Platform 2 Pose: [8.5233112301772;0.917591439939205;-1.18132378141389]
Robot Arm 2 end: [0.851065341647484;-1.68056192145179;1.25517782248529]
Robot Arm 1 togoal: 13.6758667155526

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976264e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.840633e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 247:
Mobile Platform 1 Pose: [-31.1137218837618;11.4536884888187;-0.0196778862432685]
Robot Arm 1 end: [12.2292912804223;0.181581710739409;0.79080320171316]
Robot Arm 1 togoal: 10.0947642211239
Mobile Platform 2 Pose: [8.72447252397707;0.860065436237922;-1.32041148322123]
Robot Arm 2 end: [0.952073141488276;-1.8675279969355;1.22931142243965]
Robot Arm 1 togoal: 13.6444160447934

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.431860e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 248:
Mobile Platform 1 Pose: [-31.662675171196;11.8212204997993;0.07047393774762]
Robot Arm 1 end: [12.6297478736758;0.183246699810746;0.610826746348598]
Robot Arm 1 togoal: 10.2035598977779
Mobile Platform 2 Pose: [8.73048079678724;0.904230592978003;-1.4632259910524]
Robot Arm 2 end: [1.12526847065832;-1.98039524760228;1.21213094754762]
Robot Arm 1 togoal: 13.7179101549933

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.299082e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 249:
Mobile Platform 1 Pose: [-31.6390983784506;11.8372329721512;0.135689763104437]
Robot Arm 1 end: [12.2970046478246;0.753988165443593;0.798952330558871]
Robot Arm 1 togoal: 9.55438737093228
Mobile Platform 2 Pose: [8.48907607872228;0.985031714206638;-1.59288660536674]
Robot Arm 2 end: [1.54884182991497;-1.74202943453708;1.19024200825676]
Robot Arm 1 togoal: 14.2013811413379

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.816202e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 250:
Mobile Platform 1 Pose: [-31.9179437676924;11.9938434589571;0.175107721762228]
Robot Arm 1 end: [12.7418873718653;0.307119944485496;0.412147894307809]
Robot Arm 1 togoal: 10.134072385911
Mobile Platform 2 Pose: [8.39135137530479;1.03173459949532;-1.60915441120669]
Robot Arm 2 end: [1.70494785400713;-1.53063540667739;1.1322532819399]
Robot Arm 1 togoal: 14.4529258245677

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.006923e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.767263e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 251:
Mobile Platform 1 Pose: [-31.9392973654607;12.0435313416261;0.242246264213758]
Robot Arm 1 end: [12.7790004666218;0.540384478117655;0.544851177218417]
Robot Arm 1 togoal: 9.90757467494135
Mobile Platform 2 Pose: [9.10549012311493;0.594987272011884;-1.57028696888903]
Robot Arm 2 end: [0.745951628095245;-1.70195981293349;-0.142975161054134]
Robot Arm 1 togoal: 13.678514563948

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.368461e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 252:
Mobile Platform 1 Pose: [-32.3302361553569;12.307027150843;0.347267201635745]
Robot Arm 1 end: [13.072492884308;0.113054332260787;0.636836156257434]
Robot Arm 1 togoal: 10.3910371408985
Mobile Platform 2 Pose: [9.12511555268208;0.565455449166345;-1.5995294341357]
Robot Arm 2 end: [0.903280642777684;-1.65996574313123;-0.0564206742207673]
Robot Arm 1 togoal: 13.8175958894583

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.978011e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.817990e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 253:
Mobile Platform 1 Pose: [-32.3271132796432;12.2951359419833;0.338117654592435]
Robot Arm 1 end: [13.0811024379022;0.119134241419662;0.485552324942051]
Robot Arm 1 togoal: 10.4017715923642
Mobile Platform 2 Pose: [9.07188332390983;0.601337498387573;-1.5982904439931]
Robot Arm 2 end: [1.08583382058845;-1.60479580783402;0.0957294771564542]
Robot Arm 1 togoal: 13.9787896825566

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985808e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.225279e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 254:
Mobile Platform 1 Pose: [-32.3137018446997;12.2955303158724;0.346389574791338]
Robot Arm 1 end: [13.0813214482741;0.341590032390603;0.333603496554356]
Robot Arm 1 togoal: 10.2073177390736
Mobile Platform 2 Pose: [9.26412339698731;0.468116093590968;-1.57760581307023]
Robot Arm 2 end: [0.477203258387581;-1.86327132147217;0.0680476592359499]
Robot Arm 1 togoal: 13.3450070142386

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.032979e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 255:
Mobile Platform 1 Pose: [-32.3247774726248;12.3111898390206;0.359438310073877]
Robot Arm 1 end: [13.093081381904;0.523441018302807;0.423439776245838]
Robot Arm 1 togoal: 10.0288065597774
Mobile Platform 2 Pose: [9.4711181334215;0.302520304443611;-1.87114583307554]
Robot Arm 2 end: [-0.0836871710218184;-1.73578441243963;0.163788931164188]
Robot Arm 1 togoal: 12.9796972724792

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.039729e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 256:
Mobile Platform 1 Pose: [-32.4436403249646;12.390431898469;0.379575886097508]
Robot Arm 1 end: [13.2116579871844;0.405262678469072;0.564411171957638]
Robot Arm 1 togoal: 10.1631094396233
Mobile Platform 2 Pose: [9.9308218786239;0.0416913738422836;-1.92450213905333]
Robot Arm 2 end: [-0.255285292891959;-2.39554509962101;0.083187040566451]
Robot Arm 1 togoal: 12.4440550127818

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.528295e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 257:
Mobile Platform 1 Pose: [-32.5860910727915;12.4931016980645;0.407802176675973]
Robot Arm 1 end: [13.2620136143692;0.695755779405436;0.67413655253753]
Robot Arm 1 togoal: 9.89580067682475
Mobile Platform 2 Pose: [10.2970579089394;-0.217581499569517;-2.01110538200903]
Robot Arm 2 end: [-0.2822982005592;-2.1614347057413;1.38452354081801]
Robot Arm 1 togoal: 12.485810344512

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.966544e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 258:
Mobile Platform 1 Pose: [-32.9214669070786;12.721099151847;0.483723850982732]
Robot Arm 1 end: [13.5020049309176;0.353312430072774;0.795065774664655]
Robot Arm 1 togoal: 10.2883234838079
Mobile Platform 2 Pose: [10.5040382897522;-0.360888437085828;-2.05744813702021]
Robot Arm 2 end: [-0.488270325529861;-2.39345155908375;1.34008713010929]
Robot Arm 1 togoal: 12.1805299577109

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.998041e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.748345e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 259:
Mobile Platform 1 Pose: [-32.9413674529027;12.7410744926607;0.495618282341684]
Robot Arm 1 end: [13.5431160229049;0.574909912480128;0.643231993368126]
Robot Arm 1 togoal: 10.1072484377845
Mobile Platform 2 Pose: [10.7951721416197;-0.604748330834574;-2.11251570674995]
Robot Arm 2 end: [-0.849559513769336;-2.51845329780378;1.29762446551295]
Robot Arm 1 togoal: 11.8217595466411

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.487664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 260:
Mobile Platform 1 Pose: [-33.1388780211726;12.8880601822317;0.547754472327564]
Robot Arm 1 end: [13.6318230155633;0.810492609895898;0.490785203893865]
Robot Arm 1 togoal: 9.93471323266401
Mobile Platform 2 Pose: [11.3155559992491;-1.01179993966038;-2.19904757663309]
Robot Arm 2 end: [-1.55466073621371;-2.12353889891948;1.19883014071111]
Robot Arm 1 togoal: 11.5527567348888

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.921744e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 261:
Mobile Platform 1 Pose: [-33.2324083424794;12.943682255178;0.556305050081395]
Robot Arm 1 end: [13.726350185269;0.605077926121472;0.383710312573057]
Robot Arm 1 togoal: 10.1707263413501
Mobile Platform 2 Pose: [11.8410744866013;-1.31499764761617;-2.2231305707077]
Robot Arm 2 end: [-2.0177507034704;-2.67345053604366;0.565708133189504]
Robot Arm 1 togoal: 10.8769119436787

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000009e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.944506e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 262:
Mobile Platform 1 Pose: [-33.2897720846545;12.9727621661019;0.55527050610894]
Robot Arm 1 end: [13.6749798198113;0.799868244241378;0.235995762838634]
Robot Arm 1 togoal: 9.98995179612301
Mobile Platform 2 Pose: [11.9463961660743;-1.49900860627174;-2.20102566581684]
Robot Arm 2 end: [-2.27313749531913;-2.45455002587252;0.734309644086233]
Robot Arm 1 togoal: 10.8285318115796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.956164e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 263:
Mobile Platform 1 Pose: [-33.3415166662208;13.002862093789;0.564191198956352]
Robot Arm 1 end: [13.6582457694892;0.602093450187644;0.111638507137062]
Robot Arm 1 togoal: 10.1828144167787
Mobile Platform 2 Pose: [12.2279907812363;-1.80949444289404;-2.15142782988183]
Robot Arm 2 end: [-2.06938999130652;-1.46248123008522;0.898985237184389]
Robot Arm 1 togoal: 11.6692203776344

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000938e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.865129e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 264:
Mobile Platform 1 Pose: [-33.3776731657913;13.0203414898446;0.562300110200415]
Robot Arm 1 end: [13.5708563177571;0.706106130379135;-0.00610724484964603]
Robot Arm 1 togoal: 10.0727123774639
Mobile Platform 2 Pose: [12.0978772578123;-1.73463149392934;-2.15453053775272]
Robot Arm 2 end: [-2.01181570567357;-1.45514542368897;0.734416079864566]
Robot Arm 1 togoal: 11.723665907635

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.725772e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 265:
Mobile Platform 1 Pose: [-33.3997991451912;13.0322254735798;0.566094672091977]
Robot Arm 1 end: [13.5047310416207;0.578417967080057;-0.0581040212713963]
Robot Arm 1 togoal: 10.1756040207515
Mobile Platform 2 Pose: [11.9697674624121;-1.66084848345204;-2.15896529516051]
Robot Arm 2 end: [-2.08886725455028;-1.56132812409692;0.591057079121984]
Robot Arm 1 togoal: 11.6043956405052

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.007556e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.701491e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 266:
Mobile Platform 1 Pose: [-33.4276646964413;13.0413280577803;0.558121816179329]
Robot Arm 1 end: [13.4153096103205;0.601833435330711;-0.133126996152647]
Robot Arm 1 togoal: 10.1353840889734
Mobile Platform 2 Pose: [11.9364239506253;-1.64796215980243;-2.13371540479607]
Robot Arm 2 end: [-1.82958208415633;-1.63390837333678;0.163480319071674]
Robot Arm 1 togoal: 11.7724287090375

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.754583e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 267:
Mobile Platform 1 Pose: [-33.4550166833523;13.0545916000026;0.558059061328656]
Robot Arm 1 end: [13.2679791805145;0.525411541666656;-0.191374487143669]
Robot Arm 1 togoal: 10.167582837788
Mobile Platform 2 Pose: [11.2134342070416;-1.22775895630775;-2.1741159310956]
Robot Arm 2 end: [-1.45358693752749;-1.74899699512054;0.101916683868985]
Robot Arm 1 togoal: 11.9638632673168

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.884702e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.386054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 268:
Mobile Platform 1 Pose: [-33.6362039253196;13.1409955467085;0.530714606399876]
Robot Arm 1 end: [13.4258605226754;0.391420180276492;-0.131502688702529]
Robot Arm 1 togoal: 10.3340259148597
Mobile Platform 2 Pose: [10.8544104255442;-1.00085379410479;-2.27906991276378]
Robot Arm 2 end: [-1.15760987276136;-1.93312923991673;-0.00146450402566295]
Robot Arm 1 togoal: 12.0656605781943

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.737854e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.378063e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 269:
Mobile Platform 1 Pose: [-32.5592334977818;12.5720480715994;0.56117686715939]
Robot Arm 1 end: [12.6576573538787;0.24562448620812;-0.10279853180557]
Robot Arm 1 togoal: 10.2395168805263
Mobile Platform 2 Pose: [11.1512787460644;-1.17655248984571;-2.31305279120152]
Robot Arm 2 end: [-1.14450694522066;-2.28494765903389;-0.115136921520599]
Robot Arm 1 togoal: 11.8582770204214

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.358253e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.752529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 270:
Mobile Platform 1 Pose: [-32.6554083995967;12.6316808875458;0.53880268576622]
Robot Arm 1 end: [12.5042060208027;0.165424667150119;0.522253023591758]
Robot Arm 1 togoal: 10.1974219925925
Mobile Platform 2 Pose: [11.2592133464688;-1.24930182392003;-2.31517895099777]
Robot Arm 2 end: [-1.25145843130292;-2.48362655494149;-0.0591764466576835]
Robot Arm 1 togoal: 11.6417269744387

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.010897e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.169375e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 271:
Mobile Platform 1 Pose: [-32.6981730364826;12.6556184854121;0.529582713339289]
Robot Arm 1 end: [12.4649090585639;0.121790879677839;0.431581205978747]
Robot Arm 1 togoal: 10.2392199605523
Mobile Platform 2 Pose: [12.5878616490715;-2.22138097393531;-2.33008703276949]
Robot Arm 2 end: [-2.25671832645024;-2.40022616727271;-0.0749842906605004]
Robot Arm 1 togoal: 10.9664095874891

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.002060e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.290821e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 272:
Mobile Platform 1 Pose: [-32.7262431385318;12.6658093215603;0.512826927714353]
Robot Arm 1 end: [12.3249869455339;0.15103094248322;0.353621518705511]
Robot Arm 1 togoal: 10.1867820390649
Mobile Platform 2 Pose: [12.6847753934941;-2.28931554005657;-2.33343612198567]
Robot Arm 2 end: [-2.15446700651399;-2.4205876951291;-0.0703707394416122]
Robot Arm 1 togoal: 11.0241707021189

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.988186e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.480640e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 273:
Mobile Platform 1 Pose: [-32.7756470613948;12.7552335888225;0.500979972896038]
Robot Arm 1 end: [12.617794279118;0.0447113305018359;0.258202886178993]
Robot Arm 1 togoal: 10.3708859762074
Mobile Platform 2 Pose: [12.6063966494202;-2.23952927737873;-2.35250743117757]
Robot Arm 2 end: [-2.18343069398755;-2.24413922621366;-0.0592978579916881]
Robot Arm 1 togoal: 11.1242740607208

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.336638e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.179191e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 274:
Mobile Platform 1 Pose: [-32.8512941862038;12.861923949191;0.471712633874401]
Robot Arm 1 end: [12.6537333414735;0.0710631345037466;0.188306447278372]
Robot Arm 1 togoal: 10.3635013403832
Mobile Platform 2 Pose: [11.4359002692262;-1.60180047885936;-2.40164104715489]
Robot Arm 2 end: [-2.04077856715872;-1.91303105982845;0.353030251325998]
Robot Arm 1 togoal: 11.4066833825664

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.164235e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.122192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 275:
Mobile Platform 1 Pose: [-32.9117113080469;12.9778272451844;0.446156291552484]
Robot Arm 1 end: [12.7497446798772;0.659961059355522;-0.1206719872603]
Robot Arm 1 togoal: 9.87382955752344
Mobile Platform 2 Pose: [11.5141079635983;-1.65142991212355;-2.38909595577823]
Robot Arm 2 end: [-1.61451945684333;-2.5592418363729;1.32762666179708]
Robot Arm 1 togoal: 11.2124287840401

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.411107e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.620053e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 276:
Mobile Platform 1 Pose: [-32.9782782651987;13.0249821679626;0.441282868439807]
Robot Arm 1 end: [12.668217663333;0.928699876739703;0.176449154184655]
Robot Arm 1 togoal: 9.55069046729958
Mobile Platform 2 Pose: [11.4678964673358;-1.61408702908232;-2.1770126865471]
Robot Arm 2 end: [-1.44647162280464;-2.1637857158354;1.45384335207598]
Robot Arm 1 togoal: 11.600586710257

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.161984e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.571410e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 277:
Mobile Platform 1 Pose: [-32.8094847458837;12.9218674139188;0.438251610083201]
Robot Arm 1 end: [12.5196228737896;0.969821011363619;0.591938059745618]
Robot Arm 1 togoal: 9.42102903518311
Mobile Platform 2 Pose: [11.365160522627;-1.55198830010828;-2.20158993589702]
Robot Arm 2 end: [-1.32958240443449;-2.28910137983054;1.42317548150561]
Robot Arm 1 togoal: 11.6036058382057

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.534371e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.599550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 278:
Mobile Platform 1 Pose: [-31.6940615446738;12.3452293830835;0.419325179852905]
Robot Arm 1 end: [11.981006501381;0.940480142349021;0.685296893934603]
Robot Arm 1 togoal: 9.31115924529129
Mobile Platform 2 Pose: [11.3864409090842;-1.56444596737795;-2.19474698240213]
Robot Arm 2 end: [-1.53472452002796;-2.23128538449817;1.47823840041934]
Robot Arm 1 togoal: 11.4898061029474

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.827253e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 279:
Mobile Platform 1 Pose: [-31.1277793362637;11.9966523819346;0.440569646188441]
Robot Arm 1 end: [11.7404216988457;0.54337963881354;0.899276415936889]
Robot Arm 1 togoal: 9.6355216029232
Mobile Platform 2 Pose: [11.5870492710504;-1.70360856990452;-2.18331200319804]
Robot Arm 2 end: [-2.21337484218599;-1.75363395304399;1.12807637420598]
Robot Arm 1 togoal: 11.3485009276291

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.420851e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.057006e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 280:
Mobile Platform 1 Pose: [-30.8668798455906;11.8019956102443;0.461745683390322]
Robot Arm 1 end: [11.9198274269952;0.386498207750951;0.652873369453192]
Robot Arm 1 togoal: 9.84168674088356
Mobile Platform 2 Pose: [11.7360018078286;-1.79461691890908;-2.17827541148397]
Robot Arm 2 end: [-2.2134663318543;-1.53246727253058;1.01406353513843]
Robot Arm 1 togoal: 11.5146081841867

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.251623e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.636605e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 281:
Mobile Platform 1 Pose: [-30.0806938013666;11.2912050108062;0.4818714425295]
Robot Arm 1 end: [11.3443144728911;0.397089179144747;0.615905676722099]
Robot Arm 1 togoal: 9.73869977817128
Mobile Platform 2 Pose: [11.6421268902424;-1.73157434158974;-2.21842913996128]
Robot Arm 2 end: [-2.25282597214278;-1.61008558164755;0.296530082855407]
Robot Arm 1 togoal: 11.4851511066758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.519090e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.153489e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 282:
Mobile Platform 1 Pose: [-29.6489321742502;10.9759504286758;0.454612106461359]
Robot Arm 1 end: [11.1168286766744;-0.0548696818831879;0.434596701131651]
Robot Arm 1 togoal: 10.1748701584093
Mobile Platform 2 Pose: [11.7968149495285;-1.8206293987751;-2.22363578286807]
Robot Arm 2 end: [-2.37806371123375;-1.75163317370688;0.188749485216695]
Robot Arm 1 togoal: 11.3094809573758

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.728051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.095736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 283:
Mobile Platform 1 Pose: [-29.5214931119259;10.8440955468175;0.462499195932439]
Robot Arm 1 end: [10.8322208887984;-0.0496506607016889;0.46101312676586]
Robot Arm 1 togoal: 10.1396075950366
Mobile Platform 2 Pose: [11.8114414231608;-1.8290049854776;-2.22402160738665]
Robot Arm 2 end: [-2.26084579219954;-1.64888061841692;0.246309709394482]
Robot Arm 1 togoal: 11.4568991491822

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.969996e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.165777e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 284:
Mobile Platform 1 Pose: [-29.4528631464665;10.699123158444;0.465850314625501]
Robot Arm 1 end: [10.7027092915404;0.422910047012485;0.150925258170997]
Robot Arm 1 togoal: 9.700080263288
Mobile Platform 2 Pose: [11.636656334553;-1.72628382628336;-2.23022667484546]
Robot Arm 2 end: [-2.15466130161045;-1.75601637582338;0.0929374020481281]
Robot Arm 1 togoal: 11.4696106329644

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.309925e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.618216e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 285:
Mobile Platform 1 Pose: [-28.7445162802281;10.2071131215006;0.448975214496409]
Robot Arm 1 end: [10.0643141379966;0.323917748717409;0.121453159348445]
Robot Arm 1 togoal: 9.77698498521451
Mobile Platform 2 Pose: [11.4770662185324;-1.63722125505352;-2.22578612248147]
Robot Arm 2 end: [-1.84295715972495;-1.55431123098063;0.284237951649471]
Robot Arm 1 togoal: 11.8066331799316

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.290522e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 286:
Mobile Platform 1 Pose: [-28.3318581725946;9.86725672587267;0.390100321826504]
Robot Arm 1 end: [9.89510290688451;0.123110561282435;0.0107309046700144]
Robot Arm 1 togoal: 9.99224004541041
Mobile Platform 2 Pose: [11.1303587012042;-1.44265619115314;-2.26977756793926]
Robot Arm 2 end: [-1.41365062827362;-2.25611567863065;-0.0967271036652189]
Robot Arm 1 togoal: 11.6751951117756

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.808887e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.912504e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 287:
Mobile Platform 1 Pose: [-28.0345851403498;9.65456724322415;0.449186424064746]
Robot Arm 1 end: [9.86296049223376;0.986425431711983;0.217576405870401]
Robot Arm 1 togoal: 9.10835986281345
Mobile Platform 2 Pose: [11.1040153448111;-1.44582057530527;-2.2886775897191]
Robot Arm 2 end: [-1.36475125214325;-2.25687283450941;-0.0675422109381149]
Robot Arm 1 togoal: 11.7067077179817

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.522504e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.123098e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 288:
Mobile Platform 1 Pose: [-28.1727179790526;9.73493425160804;0.45381668292649]
Robot Arm 1 end: [10.0677713978961;0.876481637853965;0.157577362032687]
Robot Arm 1 togoal: 9.22508002981602
Mobile Platform 2 Pose: [12.0972789796007;-2.09557411916303;-2.33023770596991]
Robot Arm 2 end: [-2.0910036550985;-1.66068222843786;0.542923648209583]
Robot Arm 1 togoal: 11.5348635635056

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.433829e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.232247e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 289:
Mobile Platform 1 Pose: [-28.4243693922987;9.88420640607539;0.471452461981321]
Robot Arm 1 end: [10.2905057040944;0.639088019548886;0.149799361944146]
Robot Arm 1 togoal: 9.46526586312658
Mobile Platform 2 Pose: [12.2724183098792;-2.19268899771663;-2.29079457042223]
Robot Arm 2 end: [-2.06933497159572;-1.81233931074791;1.07299186447283]
Robot Arm 1 togoal: 11.4076266876409

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.529804e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.185568e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 290:
Mobile Platform 1 Pose: [-28.8672513871119;10.1603649526542;0.51395982446543]
Robot Arm 1 end: [10.6268569039659;0.290334696387808;0.280313547248032]
Robot Arm 1 togoal: 9.80866212896858
Mobile Platform 2 Pose: [12.1674513549656;-2.13023366555145;-2.29107511748823]
Robot Arm 2 end: [-2.23966274302148;-1.64778039320594;0.906745604567053]
Robot Arm 1 togoal: 11.4175179073419

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.004718e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 291:
Mobile Platform 1 Pose: [-28.913973378219;10.1871635482019;0.509358359835211]
Robot Arm 1 end: [10.721014436362;0.396308142448686;0.406084034113418]
Robot Arm 1 togoal: 9.69503979998167
Mobile Platform 2 Pose: [12.0507765519922;-2.06920453124915;-2.29841944716916]
Robot Arm 2 end: [-2.25874314042804;-1.61093357555568;0.355028777073999]
Robot Arm 1 togoal: 11.4744490992945

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.903478e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.571577e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 292:
Mobile Platform 1 Pose: [-29.0945978675353;10.2964500958685;0.502183985274837]
Robot Arm 1 end: [10.8329461324192;0.7089622293943;0.732014584529688]
Robot Arm 1 togoal: 9.36160670505649
Mobile Platform 2 Pose: [11.5774892445538;-1.81818249988532;-2.31259845429781]
Robot Arm 2 end: [-1.94182323912924;-1.97432402617855;-0.0148042459998018]
Robot Arm 1 togoal: 11.476252969861

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.145812e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.916139e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 293:
Mobile Platform 1 Pose: [-29.4740916665874;10.5175505360404;0.489602371660215]
Robot Arm 1 end: [11.1666346760526;0.901596146820783;0.507582114611998]
Robot Arm 1 togoal: 9.22870548632576
Mobile Platform 2 Pose: [11.4028048224719;-1.72581696856717;-2.32973332997248]
Robot Arm 2 end: [-1.4994552526395;-2.64066455036736;0.0399324277176032]
Robot Arm 1 togoal: 11.3407513161944

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.809660e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.508279e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 294:
Mobile Platform 1 Pose: [-29.4499289238119;10.5344103151344;0.589690681658342]
Robot Arm 1 end: [10.7308268350447;1.11613991554306;1.12612472323681]
Robot Arm 1 togoal: 8.92261196888151
Mobile Platform 2 Pose: [11.1986401310662;-1.72607199911536;-2.58094934431472]
Robot Arm 2 end: [-1.56991888491687;-2.72123533513755;-0.058293380060141]
Robot Arm 1 togoal: 11.249037747009

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.219398e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 295:
Mobile Platform 1 Pose: [-29.6451209850181;10.6473307969449;0.605636958528504]
Robot Arm 1 end: [10.6482100071173;1.33770534451152;0.550636483069732]
Robot Arm 1 togoal: 8.74054519276805
Mobile Platform 2 Pose: [11.06017768038;-1.6467726807695;-2.65183224030375]
Robot Arm 2 end: [-1.58100330571427;-2.41705066648308;0.0169007698339066]
Robot Arm 1 togoal: 11.4299142790865

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.997571e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.574107e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 296:
Mobile Platform 1 Pose: [-29.5110416978814;10.5708737974172;0.596130382137831]
Robot Arm 1 end: [10.4678196426552;1.29880288183735;0.382591300919841]
Robot Arm 1 togoal: 8.78781320200409
Mobile Platform 2 Pose: [11.7737491075456;-2.06110484353905;-2.55305404063507]
Robot Arm 2 end: [-2.0993727736254;-2.14521019348394;1.12337891697119]
Robot Arm 1 togoal: 11.147902762419

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.016698e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 297:
Mobile Platform 1 Pose: [-29.4134926686871;10.5212751652172;0.57799138667352]
Robot Arm 1 end: [10.2845582862236;1.14985595508732;0.188734681646057]
Robot Arm 1 togoal: 8.95438182748818
Mobile Platform 2 Pose: [11.6476078895506;-1.98712947012861;-2.58499047021451]
Robot Arm 2 end: [-1.64607600610384;-2.84501933888881;1.11275289851456]
Robot Arm 1 togoal: 11.0067461155666

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.607674e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 298:
Mobile Platform 1 Pose: [-29.176142270436;10.3772783261537;0.579917935193939]
Robot Arm 1 end: [10.1184174921377;1.33599038837121;0.490613152276106]
Robot Arm 1 togoal: 8.72586857045203
Mobile Platform 2 Pose: [11.4944049897761;-1.91447523580298;-2.63643429615962]
Robot Arm 2 end: [-1.43341455739074;-3.02226505282838;0.469561713008546]
Robot Arm 1 togoal: 11.0986798138272

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.751491e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 299:
Mobile Platform 1 Pose: [-28.9160724499964;10.2281336816865;0.573556544613283]
Robot Arm 1 end: [9.94078090897385;1.32525919883397;0.492334756765601]
Robot Arm 1 togoal: 8.73571903456122
Mobile Platform 2 Pose: [10.9920696788333;-1.77135314485784;-2.75417579870916]
Robot Arm 2 end: [-2.14072429074626;-2.26186576739613;0.327189445445242]
Robot Arm 1 togoal: 11.0937874378423

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.948997e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 300:
Mobile Platform 1 Pose: [-28.8126590045574;10.1662872606253;0.575616139538017]
Robot Arm 1 end: [9.71069693370486;1.12272866124706;0.333563939215832]
Robot Arm 1 togoal: 8.96100703517539
Mobile Platform 2 Pose: [11.3494011253851;-1.98845130282732;-2.72315255261616]
Robot Arm 2 end: [-1.6080735959578;-2.46404319106243;0.0583832592726507]
Robot Arm 1 togoal: 11.3734041310106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.838119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.591580e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 301:
Mobile Platform 1 Pose: [-28.5593072473776;9.99960834194409;0.586672733962936]
Robot Arm 1 end: [9.95123890401049;0.255405570805698;-0.0524225716741303]
Robot Arm 1 togoal: 9.8709248210601
Mobile Platform 2 Pose: [11.1891980249514;-1.82161404312454;-2.61820240492949]
Robot Arm 2 end: [-1.64641094615966;-2.33459884742299;-0.0222756805636656]
Robot Arm 1 togoal: 11.4421381191012

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.601637e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.751856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 302:
Mobile Platform 1 Pose: [-28.8534533794519;10.1802900005462;0.402990130116447]
Robot Arm 1 end: [10.164480685418;0.943847391232652;0.276103200502314]
Robot Arm 1 togoal: 9.14279618117777
Mobile Platform 2 Pose: [11.6665226861025;-2.08645869632413;-2.54330862846478]
Robot Arm 2 end: [-2.05569486857429;-2.5956439707844;-0.065195518941321]
Robot Arm 1 togoal: 10.9750848950196

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.139192e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.048028e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 303:
Mobile Platform 1 Pose: [-28.3935981506341;9.9467971852494;0.313653091038827]
Robot Arm 1 end: [10.1300612215734;0.726804349403404;0.235768511431184]
Robot Arm 1 togoal: 9.36273963510732
Mobile Platform 2 Pose: [11.7044316299862;-2.11485641118974;-2.58255418183659]
Robot Arm 2 end: [-2.04126119268428;-2.60395064653438;-0.0724825024708293]
Robot Arm 1 togoal: 10.9809952157194

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.350312e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.299048e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 304:
Mobile Platform 1 Pose: [-27.9836984875383;9.73439309187359;0.148582871554529]
Robot Arm 1 end: [9.92373745862768;0.467737585768434;0.23090846857582]
Robot Arm 1 togoal: 9.61946874148675
Mobile Platform 2 Pose: [11.9087771986195;-2.24445774566941;-2.49896543382509]
Robot Arm 2 end: [-1.81574287066969;-2.68360355508482;0.0640674967205018]
Robot Arm 1 togoal: 11.0740405461274

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.297831e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.091336e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 305:
Mobile Platform 1 Pose: [-28.2462741610674;9.87768728861462;-0.0561447337631206]
Robot Arm 1 end: [9.91783221476718;-0.139668021362212;0.954113543940251]
Robot Arm 1 togoal: 10.1558347457228
Mobile Platform 2 Pose: [11.9790565313389;-2.29284559477445;-2.49040051359684]
Robot Arm 2 end: [-2.30347363404484;-2.54855406406708;-0.110689590328509]
Robot Arm 1 togoal: 10.8361882392823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.222725e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.856855e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 306:
Mobile Platform 1 Pose: [-28.188922387667;9.82841943451972;-0.110417525813854]
Robot Arm 1 end: [9.58083985064119;-0.2706477396042;1.16400291960944]
Robot Arm 1 togoal: 10.2853948459578
Mobile Platform 2 Pose: [12.0237220607852;-2.32367053173025;-2.50100177357846]
Robot Arm 2 end: [-2.32097305705351;-2.50630039809212;-0.0970545852792954]
Robot Arm 1 togoal: 10.8508566092763

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.274639e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.680682e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 307:
Mobile Platform 1 Pose: [-28.617178154044;10.2676239783884;-0.0926825301613524]
Robot Arm 1 end: [10.0240990053249;-0.319438151776207;1.09451191618276]
Robot Arm 1 togoal: 10.3282755877266
Mobile Platform 2 Pose: [11.9244861393302;-2.2561526409875;-2.55580166343817]
Robot Arm 2 end: [-2.30006294817338;-3.01843444710676;0.144080521926296]
Robot Arm 1 togoal: 10.4846170850698

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.201389e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.586813e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 308:
Mobile Platform 1 Pose: [-28.6695035550619;10.3185501133406;-0.0925401620785582]
Robot Arm 1 end: [10.1262716886916;-0.00669049755509682;0.349694686897118]
Robot Arm 1 togoal: 10.075800483819
Mobile Platform 2 Pose: [11.9798989709907;-2.20913590757952;-2.62222593138634]
Robot Arm 2 end: [-2.03277871883359;-2.77396490088467;0.0201402603999852]
Robot Arm 1 togoal: 10.8602384022944

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.230054e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.112856e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 309:
Mobile Platform 1 Pose: [-28.4406474517844;10.1983365499206;-0.176933316081971]
Robot Arm 1 end: [10.6454257275392;0.129488411070074;0.478040904564408]
Robot Arm 1 togoal: 9.94642333061092
Mobile Platform 2 Pose: [12.033366922719;-2.2643734748268;-2.65176334122746]
Robot Arm 2 end: [-2.38716833639331;-2.5579968810465;-0.0550898410290583]
Robot Arm 1 togoal: 10.7620943848268

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.371343e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.412432e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 310:
Mobile Platform 1 Pose: [-27.0137276284733;9.42349242687309;-0.11762117258674]
Robot Arm 1 end: [9.75975114125491;0.330285894574061;1.13232561101127]
Robot Arm 1 togoal: 9.68050402486655
Mobile Platform 2 Pose: [11.7742114085162;-2.09578747100852;-2.65928261121687]
Robot Arm 2 end: [-1.81471820787878;-3.03862369293626;0.0532992701437136]
Robot Arm 1 togoal: 10.8449870233855

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.296863e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.524590e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 311:
Mobile Platform 1 Pose: [-27.3411693686952;9.61023212199807;-0.172232985128809]
Robot Arm 1 end: [9.34901788031021;0.481808234989666;0.505847017933449]
Robot Arm 1 togoal: 9.5942841198635
Mobile Platform 2 Pose: [11.8582191224619;-2.13526030105188;-2.6201363588695]
Robot Arm 2 end: [-2.15900371286865;-2.65487845605625;-0.151806762104315]
Robot Arm 1 togoal: 10.8733764642569

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.183701e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.659354e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 312:
Mobile Platform 1 Pose: [-26.8262241312282;9.31740874280879;-0.172547175755129]
Robot Arm 1 end: [8.878528984534;0.219702578266224;0.87996611594834]
Robot Arm 1 togoal: 9.86523387135881
Mobile Platform 2 Pose: [11.9762418640228;-2.21848854190477;-2.59442010658282]
Robot Arm 2 end: [-2.6331888542568;-2.23797445279361;0.128882536932225]
Robot Arm 1 togoal: 10.7915215833315

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.463128e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.641541e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 313:
Mobile Platform 1 Pose: [-26.2997635936176;9.00279226674052;-0.1941954554341]
Robot Arm 1 end: [8.55518543058642;0.050650007221045;1.04568518618162]
Robot Arm 1 togoal: 10.0649380818282
Mobile Platform 2 Pose: [11.9589284577814;-2.20385934932498;-2.60652902894338]
Robot Arm 2 end: [-2.40892645568505;-2.30060718948734;0.0148912004785457]
Robot Arm 1 togoal: 10.9166574976853

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.355701e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.041479e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 314:
Mobile Platform 1 Pose: [-25.9781654347876;8.81547017092671;-0.225204154320375]
Robot Arm 1 end: [8.42570794780219;-0.0878365700967818;1.20022899835468]
Robot Arm 1 togoal: 10.2149760727126
Mobile Platform 2 Pose: [12.4175819277825;-2.45057696194568;-2.73100569652648]
Robot Arm 2 end: [-2.25883960598904;-3.12065188625369;0.0670192431411668]
Robot Arm 1 togoal: 10.4577748473193

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.727119e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.343965e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 315:
Mobile Platform 1 Pose: [-26.2268523417358;8.93705875465329;-0.167715662420012]
Robot Arm 1 end: [8.33311457671577;-0.443019498963152;0.612688536411803]
Robot Arm 1 togoal: 10.6141506012006
Mobile Platform 2 Pose: [12.3793728945012;-2.69025478687642;-2.82577674554837]
Robot Arm 2 end: [-2.797131830689;-2.26220994459447;0.204855679152813]
Robot Arm 1 togoal: 10.6530249544111

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001780e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.098865e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 316:
Mobile Platform 1 Pose: [-26.2568398603677;9.00609629267621;-0.159517640880695]
Robot Arm 1 end: [8.40539996378515;-0.341410963070516;0.762973389048438]
Robot Arm 1 togoal: 10.4910502012678
Mobile Platform 2 Pose: [11.9718936782766;-2.45249223749482;-2.83513369374428]
Robot Arm 2 end: [-2.38555802461757;-2.10915521776518;0.472938722502777]
Robot Arm 1 togoal: 11.0156066748581

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.961205e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.147596e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 317:
Mobile Platform 1 Pose: [-25.6831708009156;8.79517761855205;-0.297812956646555]
Robot Arm 1 end: [8.26148795219502;-0.431751763257182;0.866217802992971]
Robot Arm 1 togoal: 10.5958769677691
Mobile Platform 2 Pose: [11.4320190060288;-2.15992001823243;-2.84764500595826]
Robot Arm 2 end: [-2.51451074566946;-2.30110710268305;0.29854477528327]
Robot Arm 1 togoal: 10.8074001497813

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.582993e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.656442e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 318:
Mobile Platform 1 Pose: [-25.3220850061462;8.62540283688272;-0.378432510235693]
Robot Arm 1 end: [8.42208598509449;-0.603602033755665;0.876130246550236]
Robot Arm 1 togoal: 10.7397414180983
Mobile Platform 2 Pose: [11.2521164647549;-2.0478822612142;-2.88207185335312]
Robot Arm 2 end: [-2.35411242088717;-2.68108470053359;0.0503223218227265]
Robot Arm 1 togoal: 10.6859258310305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.364487e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.558967e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 319:
Mobile Platform 1 Pose: [-25.2654617889434;8.58387125802894;-0.405092824753059]
Robot Arm 1 end: [8.43492959562945;-0.508668271646902;0.766450874382798]
Robot Arm 1 togoal: 10.6513331840256
Mobile Platform 2 Pose: [11.8751141706907;-2.36581504719217;-3.00714468118591]
Robot Arm 2 end: [-2.30343376557647;-2.72536894312495;-0.0185375067759231]
Robot Arm 1 togoal: 10.7017552088099

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.497962e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.849064e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 320:
Mobile Platform 1 Pose: [-25.2278056679557;8.54871891780041;-0.381180102484043]
Robot Arm 1 end: [8.34917522910811;-0.47058924099111;0.79213176925333]
Robot Arm 1 togoal: 10.6249569587618
Mobile Platform 2 Pose: [13.0368986211255;-2.95172532823984;-3.00212662532632]
Robot Arm 2 end: [-2.73551101745837;-2.65548583024619;0.051138476827607]
Robot Arm 1 togoal: 10.4343270729111

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.410045e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.050823e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 321:
Mobile Platform 1 Pose: [-25.1151178708302;8.38753359612871;-0.206349179657152]
Robot Arm 1 end: [8.1532657274625;-0.337642776608916;0.814419385210832]
Robot Arm 1 togoal: 10.5250435540826
Mobile Platform 2 Pose: [14.0747728819889;-3.51285357100579;-2.95993551299206]
Robot Arm 2 end: [-3.68252024843899;-3.24632539625985;1.29224836874645]
Robot Arm 1 togoal: 9.25067556308128

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.134183e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.763058e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 322:
Mobile Platform 1 Pose: [-24.9958734737978;8.26528784314598;-0.16219902416775]
Robot Arm 1 end: [7.84428814967134;-0.30821816881284;0.676442549721079]
Robot Arm 1 togoal: 10.5650240266794
Mobile Platform 2 Pose: [14.0130908151275;-3.56084155382522;-2.84756084910925]
Robot Arm 2 end: [-3.8833965241899;-3.18072017449967;1.22845802416516]
Robot Arm 1 togoal: 9.16520573835598

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.809812e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.902121e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 323:
Mobile Platform 1 Pose: [-25.2065530788401;8.43896624196024;-0.143443274799574]
Robot Arm 1 end: [8.06518707830942;-0.201991380508085;0.689459436331297]
Robot Arm 1 togoal: 10.4170815912581
Mobile Platform 2 Pose: [13.9142995876936;-3.58352898636608;-2.77830186568741]
Robot Arm 2 end: [-3.9560717959511;-2.38636182943859;1.12968151229398]
Robot Arm 1 togoal: 9.72880694056306

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.852353e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.127558e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 324:
Mobile Platform 1 Pose: [-25.2145589073289;8.44633820548157;-0.143885857916318]
Robot Arm 1 end: [8.05916641915515;-0.204753794081118;0.661065492270668]
Robot Arm 1 togoal: 10.4232107502406
Mobile Platform 2 Pose: [14.2029895082966;-3.75538010168186;-2.77568534941536]
Robot Arm 2 end: [-4.47885670503876;-2.84850184573503;0.536048668273741]
Robot Arm 1 togoal: 9.08829347346465

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.122663e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.545897e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 325:
Mobile Platform 1 Pose: [-25.2207629985184;8.46888520920242;-0.158924602657621]
Robot Arm 1 end: [8.14836512649492;-0.12991340805201;0.608299823136544]
Robot Arm 1 togoal: 10.3381197019699
Mobile Platform 2 Pose: [14.6352832500712;-4.02190700489966;-2.78302308304046]
Robot Arm 2 end: [-4.21720544267746;-3.17044559822422;0.0402185189600306]
Robot Arm 1 togoal: 9.07062511702501

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.996014e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.408396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 326:
Mobile Platform 1 Pose: [-25.1591460129925;8.4477131405041;-0.189774230051242]
Robot Arm 1 end: [8.31776182465615;-0.0751166216826933;0.614447975509857]
Robot Arm 1 togoal: 10.2547421611759
Mobile Platform 2 Pose: [14.7835877305029;-4.1418310983262;-2.80171067498983]
Robot Arm 2 end: [-4.63534726931703;-3.31345588621597;0.796269202371925]
Robot Arm 1 togoal: 8.60317417209688

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.745676e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.159218e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 327:
Mobile Platform 1 Pose: [-25.2191666348083;8.48455086359807;-0.179834936194474]
Robot Arm 1 end: [8.38423278808726;-0.105606032009525;0.682932068975211]
Robot Arm 1 togoal: 10.2682196516383
Mobile Platform 2 Pose: [15.2121001253537;-4.39426627329851;-2.86228974534354]
Robot Arm 2 end: [-4.60537523648834;-3.49561370437981;0.209481651579357]
Robot Arm 1 togoal: 8.55155528544305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.952396e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.043554e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 328:
Mobile Platform 1 Pose: [-25.2141520592535;8.47473689100059;-0.152242321372833]
Robot Arm 1 end: [8.40825704980444;-0.130161449396761;0.706706869536653]
Robot Arm 1 togoal: 10.2867336853183
Mobile Platform 2 Pose: [15.5022557669416;-4.62855562619585;-2.94951645657026]
Robot Arm 2 end: [-5.05532099430849;-3.61311320840961;0.645498727968708]
Robot Arm 1 togoal: 8.12457234783755

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.502887e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.092390e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 329:
Mobile Platform 1 Pose: [-24.5641882004981;8.03208656389083;-0.16164818976774]
Robot Arm 1 end: [8.23325340182085;-0.395275025965845;0.487717357678371]
Robot Arm 1 togoal: 10.5948482493413
Mobile Platform 2 Pose: [16.2655267594357;-5.13476321528796;-3.07408854941528]
Robot Arm 2 end: [-5.73366683861753;-3.61196914763994;0.522475480471125]
Robot Arm 1 togoal: 7.74632738984843

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.925248e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.966204e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 330:
Mobile Platform 1 Pose: [-24.6508521609714;8.07410803752761;-0.195902207589902]
Robot Arm 1 end: [8.2841318488557;-0.443656736716017;0.514847972538028]
Robot Arm 1 togoal: 10.6313927332608
Mobile Platform 2 Pose: [17.297557390724;-5.73568104147322;-3.15899105995188]
Robot Arm 2 end: [-5.68838708101221;-3.41512960843932;1.38026887045251]
Robot Arm 1 togoal: 7.87212355642737

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.826861e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.752937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 331:
Mobile Platform 1 Pose: [-24.866057152423;8.18816811619304;-0.260312042563005]
Robot Arm 1 end: [8.34191724626092;-0.302478285727484;0.417154264016146]
Robot Arm 1 togoal: 10.4932727332793
Mobile Platform 2 Pose: [17.4846482295285;-5.89438038023752;-3.19482830573486]
Robot Arm 2 end: [-6.39904263865945;-3.78928180931708;0.717692352138219]
Robot Arm 1 togoal: 7.22393366105931

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.432606e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.827770e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 332:
Mobile Platform 1 Pose: [-24.1451888472648;7.74183756740712;-0.194211481803547]
Robot Arm 1 end: [7.84789226638573;-0.261711453200402;0.510599143995252]
Robot Arm 1 togoal: 10.5335274023302
Mobile Platform 2 Pose: [17.8382014774337;-6.21092372527071;-3.23048122739891]
Robot Arm 2 end: [-6.86165141497261;-2.83237120710512;0.803927657765419]
Robot Arm 1 togoal: 7.85737405867106

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.368550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 333:
Mobile Platform 1 Pose: [-23.0185553629422;7.03355904865979;-0.122315485310458]
Robot Arm 1 end: [6.70676296465167;-0.0403443557187217;1.03222492209456]
Robot Arm 1 togoal: 10.5779405381759
Mobile Platform 2 Pose: [17.7279772792122;-6.14875820414143;-3.22582905772871]
Robot Arm 2 end: [-6.90635787039896;-3.02651318968574;0.64575459657903]
Robot Arm 1 togoal: 7.67894487777317

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.133191e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.514629e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 334:
Mobile Platform 1 Pose: [-23.0302851630669;7.04299124536642;-0.124033438493451]
Robot Arm 1 end: [6.77308147649843;-0.130941453570068;0.912155052461029]
Robot Arm 1 togoal: 10.6498671382851
Mobile Platform 2 Pose: [17.9160668680928;-6.25941494769188;-3.2281575352247]
Robot Arm 2 end: [-7.05193753001819;-3.19828509790797;0.485424856255686]
Robot Arm 1 togoal: 7.48510613248717

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.922106e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.224157e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 335:
Mobile Platform 1 Pose: [-23.0372635308204;7.02069179083671;-0.0805060878372765]
Robot Arm 1 end: [6.8309743676412;-0.201777452897919;0.852795439034638]
Robot Arm 1 togoal: 10.7035267081602
Mobile Platform 2 Pose: [18.0796425470484;-6.35788891801676;-3.23062249678976]
Robot Arm 2 end: [-6.83600336515025;-3.89277272129879;0.596390492247019]
Robot Arm 1 togoal: 6.94002899712097

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.022837e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.127143e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 336:
Mobile Platform 1 Pose: [-22.8896135487565;6.89790584665423;-0.0635099507176256]
Robot Arm 1 end: [6.7984990285658;-0.227782605517322;0.795172089771736]
Robot Arm 1 togoal: 10.7416931463845
Mobile Platform 2 Pose: [18.9230170145596;-6.88011720211229;-3.31014210885478]
Robot Arm 2 end: [-7.13039638662429;-3.86409058893145;1.16547333194175]
Robot Arm 1 togoal: 6.78309725792275

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.070804e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 337:
Mobile Platform 1 Pose: [-22.8033361763249;6.829322419697;-0.0626286507500319]
Robot Arm 1 end: [6.76962896209314;-0.0950670566198995;0.705112334569767]
Robot Arm 1 togoal: 10.6306795927072
Mobile Platform 2 Pose: [19.2805048947839;-7.12664631224688;-3.37280081220884]
Robot Arm 2 end: [-7.80015821175058;-3.67549064701598;0.247394696152292]
Robot Arm 1 togoal: 6.81621543961896

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.413835e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.317667e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 338:
Mobile Platform 1 Pose: [-22.2626805617448;6.45030147484217;-0.079579063316822]
Robot Arm 1 end: [6.36493471500817;-0.145820325687711;0.679825527846829]
Robot Arm 1 togoal: 10.8101315533111
Mobile Platform 2 Pose: [19.5236120530346;-7.27469319607151;-3.38626640406858]
Robot Arm 2 end: [-7.86468922773592;-3.93626482174111;0.504261136646424]
Robot Arm 1 togoal: 6.50862461065738

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.286908e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.316840e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 339:
Mobile Platform 1 Pose: [-22.1224695676062;6.34575168532648;-0.0664323173523763]
Robot Arm 1 end: [6.21780205397481;-0.136901371249647;0.645952747125129]
Robot Arm 1 togoal: 10.8548375578875
Mobile Platform 2 Pose: [20.3095902432313;-7.75324771815774;-3.4431172430102]
Robot Arm 2 end: [-7.87361309096458;-4.07749711855065;1.10093428561986]
Robot Arm 1 togoal: 6.30666448069744

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.126987e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 340:
Mobile Platform 1 Pose: [-22.2017841277434;6.39783715874851;-0.05493174823894]
Robot Arm 1 end: [6.32803162527174;-0.0595187784205272;0.64096888002602]
Robot Arm 1 togoal: 10.7448464191222
Mobile Platform 2 Pose: [20.4579459561763;-7.89347582426428;-3.49650367897699]
Robot Arm 2 end: [-8.44834149250498;-3.80230262611044;0.0919690533322684]
Robot Arm 1 togoal: 6.54684857841796

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.877827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.703011e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 341:
Mobile Platform 1 Pose: [-22.6028681333225;6.65243803964168;-0.0414302435796289]
Robot Arm 1 end: [6.68968055688569;-0.0306958541958329;0.676150847125311]
Robot Arm 1 togoal: 10.5965486940775
Mobile Platform 2 Pose: [20.5889050734479;-7.97745517731444;-3.50328314705978]
Robot Arm 2 end: [-8.58507827028767;-4.0397225187867;0.41318733373003]
Robot Arm 1 togoal: 6.22528393391166

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.395915e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.172018e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 342:
Mobile Platform 1 Pose: [-22.9775566173557;6.90237474901304;-0.0357950184865917]
Robot Arm 1 end: [6.86710620086284;-0.0531650174096882;0.715477860175238]
Robot Arm 1 togoal: 10.560777260301
Mobile Platform 2 Pose: [20.8937626999114;-8.17924827240471;-3.52572214221513]
Robot Arm 2 end: [-8.64774998358925;-4.11371663186233;0.940390162457535]
Robot Arm 1 togoal: 6.06745579137978

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.364949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.281894e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 343:
Mobile Platform 1 Pose: [-23.0894034643855;6.97414714411832;0.0657841847006385]
Robot Arm 1 end: [6.90584636231559;-0.0290917576685854;0.741734848667549]
Robot Arm 1 togoal: 10.5244345403168
Mobile Platform 2 Pose: [21.3612710858643;-8.51550097479904;-3.59780911705091]
Robot Arm 2 end: [-9.0189258654096;-4.06476135770782;0.137188410409387]
Robot Arm 1 togoal: 6.17288414875947

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.401647e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.532374e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 344:
Mobile Platform 1 Pose: [-22.9150759926691;6.83483529066531;0.161394541895838]
Robot Arm 1 end: [6.63730240038069;-0.0345436621472579;0.645118048322373]
Robot Arm 1 togoal: 10.6191793867182
Mobile Platform 2 Pose: [21.4979516586441;-8.60436377144116;-3.6090802958402]
Robot Arm 2 end: [-9.16761600683679;-4.18666338375274;0.493101072482523]
Robot Arm 1 togoal: 5.96190589753537

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.052128e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.307604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 345:
Mobile Platform 1 Pose: [-23.0484237908234;6.93866718843002;0.189547894761212]
Robot Arm 1 end: [6.82868312433033;0.157863775967525;0.549886971535459]
Robot Arm 1 togoal: 10.3859499658131
Mobile Platform 2 Pose: [21.8587712859259;-8.85556201983763;-3.63754527522309]
Robot Arm 2 end: [-9.54759545352915;-3.92535932447268;0.979512893705683]
Robot Arm 1 togoal: 6.11548342297823

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.753939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.242288e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 346:
Mobile Platform 1 Pose: [-23.4073609547562;7.16601564851851;0.217290408996199]
Robot Arm 1 end: [7.37246277523369;0.21114306438246;0.44512525024375]
Robot Arm 1 togoal: 10.1923097701494
Mobile Platform 2 Pose: [21.6120039417493;-8.70072069889308;-3.62850124972493]
Robot Arm 2 end: [-9.45624572208134;-3.90565444325359;0.936525335649025]
Robot Arm 1 togoal: 6.14640765919881

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.691996e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.573529e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 347:
Mobile Platform 1 Pose: [-22.5954321995278;6.6674247445497;0.243602691847178]
Robot Arm 1 end: [6.86084719894633;0.361912075457528;0.357752925176789]
Robot Arm 1 togoal: 10.2029487358635
Mobile Platform 2 Pose: [21.7955679267832;-8.8111799442105;-3.63695040919849]
Robot Arm 2 end: [-9.30681396399466;-4.25992598748518;0.934152876519685]
Robot Arm 1 togoal: 5.811483983975

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.181574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.233664e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 348:
Mobile Platform 1 Pose: [-22.1750666513924;6.42330755249368;0.299746613439219]
Robot Arm 1 end: [6.50986780444522;0.535714132259897;0.285725522750289]
Robot Arm 1 togoal: 10.1626587447822
Mobile Platform 2 Pose: [22.3457226004422;-9.11743936829857;-3.71929132825932]
Robot Arm 2 end: [-9.10593676536857;-3.98367383645278;1.39092323360086]
Robot Arm 1 togoal: 6.08378579002083

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.038975e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.803947e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 349:
Mobile Platform 1 Pose: [-22.4888909466903;6.61709262609624;0.353852449772098]
Robot Arm 1 end: [6.2321530866115;0.387296150741469;0.449532050041035]
Robot Arm 1 togoal: 10.3802114248251
Mobile Platform 2 Pose: [22.4122612749912;-9.16600410584931;-3.74285529845595]
Robot Arm 2 end: [-9.00819063958828;-4.17328381611232;1.30903721807037]
Robot Arm 1 togoal: 5.91432460352847

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.486303e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 350:
Mobile Platform 1 Pose: [-23.0445382338554;6.93430123829031;0.453563663725494]
Robot Arm 1 end: [7.09754656973711;0.454069504052582;0.335555617522673]
Robot Arm 1 togoal: 10.0476018697125
Mobile Platform 2 Pose: [22.447953196993;-9.22381581736056;-3.82558043058847]
Robot Arm 2 end: [-9.18307975854882;-4.3770330570028;1.27162301757175]
Robot Arm 1 togoal: 5.68746910341471

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.667379e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 351:
Mobile Platform 1 Pose: [-22.9549201453561;6.89890633785407;0.545130435394727]
Robot Arm 1 end: [6.4415261952091;0.793094855228405;0.717512824301117]
Robot Arm 1 togoal: 9.90330398431338
Mobile Platform 2 Pose: [22.6674383469808;-9.44117866409492;-3.94734881741305]
Robot Arm 2 end: [-10.2148967345711;-3.78117111047022;0.292916022382274]
Robot Arm 1 togoal: 6.34257074220389

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.334604e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.958836e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 352:
Mobile Platform 1 Pose: [-23.1341734582501;7.00052503804914;0.562153616344893]
Robot Arm 1 end: [6.58756247359941;0.413857027807547;0.59080532653295]
Robot Arm 1 togoal: 10.21783387443
Mobile Platform 2 Pose: [22.6888688137513;-9.45356950052051;-3.94505159664468]
Robot Arm 2 end: [-10.2615170184873;-3.89482756124479;0.301270000652397]
Robot Arm 1 togoal: 6.23131309831269

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.322775e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.985796e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 353:
Mobile Platform 1 Pose: [-23.3695825103375;6.9756792877977;0.722626893795441]
Robot Arm 1 end: [7.58390898845578;0.349369410101876;0.754335221157911]
Robot Arm 1 togoal: 9.97797281224293
Mobile Platform 2 Pose: [22.8080481214842;-9.51859293328874;-3.95328047805558]
Robot Arm 2 end: [-10.3346032107001;-4.16520473551197;0.842489580256095]
Robot Arm 1 togoal: 5.88363590620662

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.889917e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 354:
Mobile Platform 1 Pose: [-23.1344426691715;6.82444082313119;0.733622558606474]
Robot Arm 1 end: [7.08146916498982;0.436872863004562;1.26118481548956]
Robot Arm 1 togoal: 10.0019361527329
Mobile Platform 2 Pose: [22.941251266832;-9.59187256945596;-3.96302338297358]
Robot Arm 2 end: [-10.1353103682985;-4.38131938465183;1.161500977314]
Robot Arm 1 togoal: 5.63179547749833

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.040696e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 355:
Mobile Platform 1 Pose: [-23.1063688777419;6.80126983204151;0.732736440177245]
Robot Arm 1 end: [6.8842281606654;0.517160435486599;1.29119169044932]
Robot Arm 1 togoal: 9.9842421955915
Mobile Platform 2 Pose: [23.2201012564067;-9.75097146447014;-3.9903546390976]
Robot Arm 2 end: [-9.76342003517234;-4.14146228177791;1.46236298467409]
Robot Arm 1 togoal: 5.8636057656663

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.678070e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.530250e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 356:
Mobile Platform 1 Pose: [-23.0377863674899;6.75600223909547;0.74807580257481]
Robot Arm 1 end: [6.6394440804243;0.814548805680247;1.3739199574155]
Robot Arm 1 togoal: 9.78199786703882
Mobile Platform 2 Pose: [22.8980281973838;-9.50165157311284;-4.00898310341161]
Robot Arm 2 end: [-9.51114447037377;-4.06422946327606;1.48947187059026]
Robot Arm 1 togoal: 5.95595043770768

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.209955e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.609317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 357:
Mobile Platform 1 Pose: [-22.703936208325;6.52980803913252;0.751903586821518]
Robot Arm 1 end: [6.24217843185338;0.498110917981545;1.22046374798019]
Robot Arm 1 togoal: 10.2223989896593
Mobile Platform 2 Pose: [22.4812913768643;-9.21122767413759;-4.03915823874706]
Robot Arm 2 end: [-8.47557580679709;-3.71899541524218;0.816895803800237]
Robot Arm 1 togoal: 6.50158830084443

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.350014e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.687020e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 358:
Mobile Platform 1 Pose: [-22.7383809698032;6.55760217602858;0.760643398174913]
Robot Arm 1 end: [6.37484471139647;0.823908113443222;1.19856274273793]
Robot Arm 1 togoal: 9.87149324884001
Mobile Platform 2 Pose: [22.2620614282654;-9.06200590342879;-4.03843122837452]
Robot Arm 2 end: [-8.28266024615365;-3.84585418815751;0.884755182374442]
Robot Arm 1 togoal: 6.42087018802863

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.647421e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.982397e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 359:
Mobile Platform 1 Pose: [-22.7805081351279;6.58439707656658;0.765805070096933]
Robot Arm 1 end: [6.60446567468174;1.02042923926211;1.03086513547776]
Robot Arm 1 togoal: 9.61262589446472
Mobile Platform 2 Pose: [21.9500365548524;-8.84675010867584;-4.05026838836196]
Robot Arm 2 end: [-8.07160485502338;-3.95770629622883;0.961994982647759]
Robot Arm 1 togoal: 6.36714281661933

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.218868e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.008671e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 360:
Mobile Platform 1 Pose: [-23.0271794511401;6.73905472612104;0.752604719379917]
Robot Arm 1 end: [6.96890832147542;0.976462965644022;0.783368458892442]
Robot Arm 1 togoal: 9.5475566332152
Mobile Platform 2 Pose: [21.701729066921;-8.68992316330835;-4.06849201653081]
Robot Arm 2 end: [-8.67236276455874;-4.35221130860059;1.4116443864483]
Robot Arm 1 togoal: 5.80276626979275

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.608550e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.919621e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 361:
Mobile Platform 1 Pose: [-22.5863630888186;6.46982573941378;0.730371492367516]
Robot Arm 1 end: [6.40260984474234;0.717060990172035;0.628040806917606]
Robot Arm 1 togoal: 9.99557645710546
Mobile Platform 2 Pose: [21.9080006578348;-8.77286592571493;-4.20908998925819]
Robot Arm 2 end: [-8.55430757828735;-4.99165466158336;0.355183625937634]
Robot Arm 1 togoal: 5.34159877050579

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.439283e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.117574e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 362:
Mobile Platform 1 Pose: [-22.2976012610325;6.28182325660397;0.627822251382063]
Robot Arm 1 end: [6.12622625118885;0.603652681946075;0.585538212291743]
Robot Arm 1 togoal: 10.2064957127674
Mobile Platform 2 Pose: [22.0755649024835;-8.85504469029783;-4.22005549404594]
Robot Arm 2 end: [-8.5311244505932;-4.6796273803079;1.31674566752418]
Robot Arm 1 togoal: 5.52319472989588

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.382394e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 363:
Mobile Platform 1 Pose: [-22.3135449601563;6.30070359184124;0.60459108396191]
Robot Arm 1 end: [6.07771751600067;0.532189960296693;0.629434085227841]
Robot Arm 1 togoal: 10.2868176134594
Mobile Platform 2 Pose: [22.0786135985824;-8.86657957183745;-4.2446936713849]
Robot Arm 2 end: [-8.55953227251783;-4.57086044993711;1.39756130746931]
Robot Arm 1 togoal: 5.6183396692251

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.853704e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.010123e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 364:
Mobile Platform 1 Pose: [-22.3794655051064;6.36529419455569;0.576146809079865]
Robot Arm 1 end: [6.08641739691835;0.477412883630246;0.719340694278276]
Robot Arm 1 togoal: 10.3265895542742
Mobile Platform 2 Pose: [22.1668116674272;-8.91180874100733;-4.2916964212257]
Robot Arm 2 end: [-9.06366070789496;-4.16309644607182;1.47823457144272]
Robot Arm 1 togoal: 5.91168362225846

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.082739e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.468563e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 365:
Mobile Platform 1 Pose: [-22.4179042637194;6.41488056317761;0.550147109800028]
Robot Arm 1 end: [6.10697793634393;0.442410531208883;0.830309362136076]
Robot Arm 1 togoal: 10.3431228746106
Mobile Platform 2 Pose: [21.454941389475;-8.4713090497891;-4.4056796384348]
Robot Arm 2 end: [-8.8078114014795;-4.27614602255993;1.39740399977005]
Robot Arm 1 togoal: 5.84799914336276

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.381105e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.523150e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 366:
Mobile Platform 1 Pose: [-22.333400298748;6.35904860763255;0.536243039680167]
Robot Arm 1 end: [6.04294510006068;0.307825237874829;0.793661673163857]
Robot Arm 1 togoal: 10.4940724288712
Mobile Platform 2 Pose: [21.6230321308411;-8.5388050829005;-4.47738473121543]
Robot Arm 2 end: [-8.87182070498824;-4.59500029527946;1.43688476779024]
Robot Arm 1 togoal: 5.52212691831798

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.446443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 367:
Mobile Platform 1 Pose: [-22.1872002585194;6.2582110710705;0.500046242646132]
Robot Arm 1 end: [5.98726976891405;0.254865595777065;0.678680262645457]
Robot Arm 1 togoal: 10.572565961193
Mobile Platform 2 Pose: [21.6939999277027;-8.58473739147033;-4.47571808380025]
Robot Arm 2 end: [-8.77106005302595;-4.51009243354013;1.49955448583244]
Robot Arm 1 togoal: 5.6258189085341

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.513978e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.986107e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 368:
Mobile Platform 1 Pose: [-22.0532988959833;6.16881763398751;0.449878962855382]
Robot Arm 1 end: [6.2127379331698;0.528551055900282;0.809895951443822]
Robot Arm 1 togoal: 10.225329727271
Mobile Platform 2 Pose: [21.7166770722191;-8.59524311459007;-4.47918275058169]
Robot Arm 2 end: [-8.92324688616508;-4.44708391858143;1.44860105258594]
Robot Arm 1 togoal: 5.65681145903033

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.005691e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 369:
Mobile Platform 1 Pose: [-22.0132096202167;6.14347248817371;0.462623754948616]
Robot Arm 1 end: [6.13666879695238;0.522876130793363;0.868483471824535]
Robot Arm 1 togoal: 10.255095447448
Mobile Platform 2 Pose: [21.6995081391852;-8.58794574555399;-4.46556545125304]
Robot Arm 2 end: [-8.84565601235224;-4.37233743221925;1.46412360327761]
Robot Arm 1 togoal: 5.74511365798131

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.972051e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.976099e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 370:
Mobile Platform 1 Pose: [-21.9410573437725;6.09662062178228;0.459435922681173]
Robot Arm 1 end: [6.03095087222737;0.470094895158367;0.901248214870717]
Robot Arm 1 togoal: 10.3419792380922
Mobile Platform 2 Pose: [21.7659605584085;-8.61349122552325;-4.50624575640612]
Robot Arm 2 end: [-8.98574217134583;-4.37677086207857;1.40036061564903]
Robot Arm 1 togoal: 5.71524091719775

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.293145e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 371:
Mobile Platform 1 Pose: [-22.0084150006153;6.14865346787527;0.482528177886969]
Robot Arm 1 end: [6.02364509332732;0.42718386471149;0.673259834950024]
Robot Arm 1 togoal: 10.4004264571843
Mobile Platform 2 Pose: [21.8414900605344;-8.65212405705583;-4.51912604993405]
Robot Arm 2 end: [-9.10015824322982;-4.26341590618609;1.29464746781068]
Robot Arm 1 togoal: 5.81113996746374

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.855766e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.000000e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 372:
Mobile Platform 1 Pose: [-21.3228892293557;5.65799492302996;0.334632518671531]
Robot Arm 1 end: [5.49851293212897;0.212312991125697;0.664983112996021]
Robot Arm 1 togoal: 10.8071720499274
Mobile Platform 2 Pose: [21.8603509407423;-8.66200104686418;-4.51598949019604]
Robot Arm 2 end: [-9.15351532401382;-4.17873903758801;1.17117577474506]
Robot Arm 1 togoal: 5.89287641884305

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.779816e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.001050e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 373:
Mobile Platform 1 Pose: [-20.8824295190547;5.38728870519063;0.113217041168558]
Robot Arm 1 end: [5.66652644954835;-0.205791384668654;0.227793247518952]
Robot Arm 1 togoal: 11.1628649774376
Mobile Platform 2 Pose: [21.655958028134;-8.54742204885095;-4.50274249905299]
Robot Arm 2 end: [-9.08199461825483;-4.20488278518108;1.07517532041538]
Robot Arm 1 togoal: 5.88429070146356

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.268088e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.020738e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 374:
Mobile Platform 1 Pose: [-20.7742588787718;5.32129030554399;0.129617691520998]
Robot Arm 1 end: [5.24541934769056;-0.351212864933675;0.159098114092184]
Robot Arm 1 togoal: 11.4720713779737
Mobile Platform 2 Pose: [21.5337223863043;-8.47956134219502;-4.4964809693287]
Robot Arm 2 end: [-9.03349212492544;-4.26592866402752;1.00268441872279]
Robot Arm 1 togoal: 5.83801015761339

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.575340e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.015243e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 375:
Mobile Platform 1 Pose: [-20.6181698495149;5.25984834796879;0.274573742487675]
Robot Arm 1 end: [5.43415126082378;-0.246637776942624;0.575451562496009]
Robot Arm 1 togoal: 11.2576472803411
Mobile Platform 2 Pose: [21.377912576616;-8.39388962276609;-4.4910528219561]
Robot Arm 2 end: [-8.91761717975143;-4.2133331230462;0.731807435341423]
Robot Arm 1 togoal: 5.93968778796629

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.842004e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.960065e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 376:
Mobile Platform 1 Pose: [-20.75471555977;5.2707796222203;0.226959697971448]
Robot Arm 1 end: [5.03264167872925;-0.186676181816675;0.839030091729756]
Robot Arm 1 togoal: 11.3537704433758
Mobile Platform 2 Pose: [21.3195657551744;-8.36308074658484;-4.48121545988022]
Robot Arm 2 end: [-8.90156430269101;-4.11938760618264;0.7802383850349]
Robot Arm 1 togoal: 6.02800886509357

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.365733e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.247827e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 377:
Mobile Platform 1 Pose: [-20.7251276318285;5.16662720972222;0.225806408648098]
Robot Arm 1 end: [4.5917322671873;0.171949428305541;0.705022173643377]
Robot Arm 1 togoal: 11.2474778382832
Mobile Platform 2 Pose: [21.1418507282;-8.27449683534042;-4.4701003833987]
Robot Arm 2 end: [-8.46773290025605;-3.95062458220529;0.298640563595413]
Robot Arm 1 togoal: 6.35900526829031

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.290318e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.343686e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 378:
Mobile Platform 1 Pose: [-20.2010637433776;4.87861275800661;0.140319445772988]
Robot Arm 1 end: [4.16259642680108;0.0482573780052469;0.700788024855149]
Robot Arm 1 togoal: 11.5665556400768
Mobile Platform 2 Pose: [20.3507446388349;-7.85749185610588;-4.43436108966186]
Robot Arm 2 end: [-8.02757168359868;-3.93641726597783;0.355730470832723]
Robot Arm 1 togoal: 6.48192580266693

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.600443e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.296255e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 379:
Mobile Platform 1 Pose: [-20.3367279562234;4.89808564544637;0.171957834845595]
Robot Arm 1 end: [4.14671779453638;0.44632631068351;0.720335780380021]
Robot Arm 1 togoal: 11.2327492953112
Mobile Platform 2 Pose: [19.3839744101312;-7.31513009317258;-4.4252632537399]
Robot Arm 2 end: [-7.52343231211468;-3.96663653544633;0.240652918131449]
Robot Arm 1 togoal: 6.646363732007

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.267317e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.452210e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 380:
Mobile Platform 1 Pose: [-19.9410269064183;4.65089242994126;0.15804046156313]
Robot Arm 1 end: [4.03800558221133;0.510060082316172;0.210528748903831]
Robot Arm 1 togoal: 11.2836905300944
Mobile Platform 2 Pose: [18.3903184341342;-6.76185355695395;-4.41688571509355]
Robot Arm 2 end: [-7.25250811335046;-4.33613453206727;0.246218799385961]
Robot Arm 1 togoal: 6.42286157530152

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.193643e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.690231e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 381:
Mobile Platform 1 Pose: [-19.5839141959592;4.45790419661469;0.124766673926607]
Robot Arm 1 end: [4.03891099691871;0.375260094399078;0.16539579721388]
Robot Arm 1 togoal: 11.4020990658622
Mobile Platform 2 Pose: [18.4668164214107;-6.80437303478108;-4.42538741102854]
Robot Arm 2 end: [-7.08347282031824;-4.08107341903217;0.266399510880464]
Robot Arm 1 togoal: 6.7166840813015

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.611087e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.434387e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 382:
Mobile Platform 1 Pose: [-19.6179587641734;4.49771267082876;0.102819334640042]
Robot Arm 1 end: [4.31776709493264;0.275680884499302;0.0422631872416775]
Robot Arm 1 togoal: 11.3594372927944
Mobile Platform 2 Pose: [18.6011606785331;-6.88300106004746;-4.43577031703558]
Robot Arm 2 end: [-6.82814717280925;-4.08122133267236;0.256802189042066]
Robot Arm 1 togoal: 6.83306573767599

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','5.935003e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.572494e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 383:
Mobile Platform 1 Pose: [-19.7234648285168;4.56527515077688;0.105342734735088]
Robot Arm 1 end: [4.49126240737465;0.226690007776461;0.0310784082135963]
Robot Arm 1 togoal: 11.3174044912326
Mobile Platform 2 Pose: [19.019413348485;-7.13231063372667;-4.41716743903352]
Robot Arm 2 end: [-6.99573570897647;-4.04720852537991;0.217453516167784]
Robot Arm 1 togoal: 6.79415658534156

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.371674e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.834341e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 384:
Mobile Platform 1 Pose: [-19.6680342105693;4.51065985992188;0.203532638154454]
Robot Arm 1 end: [4.42732900214191;0.252989367171235;0.537425502947396]
Robot Arm 1 togoal: 11.2705943551393
Mobile Platform 2 Pose: [19.4401268856431;-7.38196050790977;-4.40387338898436]
Robot Arm 2 end: [-7.16337077585749;-4.05831206890381;0.148483488828038]
Robot Arm 1 togoal: 6.72561687871329

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.847939e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.197414e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 385:
Mobile Platform 1 Pose: [-19.6569176690271;4.50843655161344;-0.153355714901474]
Robot Arm 1 end: [4.66465541590364;0.0542317050022212;-0.0378189513854373]
Robot Arm 1 togoal: 11.3936001918633
Mobile Platform 2 Pose: [19.8774564937912;-7.63011511310837;-4.37297290704884]
Robot Arm 2 end: [-7.29268262698731;-4.15033757593494;0.0645827660862224]
Robot Arm 1 togoal: 6.60827276926248

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.372676e-02','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 386:
Mobile Platform 1 Pose: [-19.6120297448144;4.47462198936902;-0.211138418642707]
Robot Arm 1 end: [4.84953799747092;-0.241463508227092;-0.0407257114985943]
Robot Arm 1 togoal: 11.5695212013203
Mobile Platform 2 Pose: [20.0466495220913;-7.64115933795341;-4.22299024062929]
Robot Arm 2 end: [-7.25819056665138;-4.17686954561845;0.0394097647762423]
Robot Arm 1 togoal: 6.60465569751034

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.813937e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.803257e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 387:
Mobile Platform 1 Pose: [-19.731262474422;4.54247530934929;-0.227132113248248]
Robot Arm 1 end: [4.86319357289612;-0.10498665756749;-0.0449321212485356]
Robot Arm 1 togoal: 11.4433246492228
Mobile Platform 2 Pose: [20.3703290877669;-7.79581170772872;-4.13759302253423]
Robot Arm 2 end: [-7.45727483283885;-4.21277387207614;0.00612590065372981]
Robot Arm 1 togoal: 6.50017546442475

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.543998e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 388:
Mobile Platform 1 Pose: [-19.9961837883708;4.6969388450865;-0.257553361611415]
Robot Arm 1 end: [4.92169937207662;-0.194004939058052;-0.0292017303993677]
Robot Arm 1 togoal: 11.4939114043062
Mobile Platform 2 Pose: [19.9386864444062;-7.53550037789858;-4.22895233665549]
Robot Arm 2 end: [-7.17166165337222;-3.86424985752635;0.583908055899622]
Robot Arm 1 togoal: 6.82092874362197

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.890109e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.430703e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 389:
Mobile Platform 1 Pose: [-20.1625097216101;4.81364645268392;-0.223859162159189]
Robot Arm 1 end: [5.43953233016923;0.0184721289463557;0.661318397168255]
Robot Arm 1 togoal: 11.007625378184
Mobile Platform 2 Pose: [19.2907492022256;-7.2370679411119;-4.35616954441758]
Robot Arm 2 end: [-7.2358700578349;-4.20364806051551;0.665784394389269]
Robot Arm 1 togoal: 6.47838744408046

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.865034e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.157298e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 390:
Mobile Platform 1 Pose: [-20.7094587926352;5.12750845928873;-0.212229175631905]
Robot Arm 1 end: [5.68241310396861;-0.26082757393142;1.14672104785037]
Robot Arm 1 togoal: 11.1385018669817
Mobile Platform 2 Pose: [19.2034826628497;-7.18986724998518;-4.46427207265708]
Robot Arm 2 end: [-7.21185510618389;-4.5013074926241;0.640379132175878]
Robot Arm 1 togoal: 6.2277495415625

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.208380e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.243094e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 391:
Mobile Platform 1 Pose: [-21.051740395452;5.41535522725985;-0.126430097580998]
Robot Arm 1 end: [6.18706828210091;0.236171435264224;0.49104915337667]
Robot Arm 1 togoal: 10.5324069078901
Mobile Platform 2 Pose: [18.8691383944457;-6.95147042548685;-4.5395705117233]
Robot Arm 2 end: [-6.97450510300179;-4.66434019899884;0.627797290397145]
Robot Arm 1 togoal: 6.19844302742422

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','6.027772e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.683703e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 392:
Mobile Platform 1 Pose: [-21.6026703948337;5.71083741545568;-0.141879959399863]
Robot Arm 1 end: [6.35113422895063;0.231877611005885;0.944816861100371]
Robot Arm 1 togoal: 10.4432956211656
Mobile Platform 2 Pose: [18.6601003327153;-6.80507578678615;-4.49789595812577]
Robot Arm 2 end: [-6.77194989113191;-4.70199306454669;0.563410704986392]
Robot Arm 1 togoal: 6.27743277553805

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.467039e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.379355e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 393:
Mobile Platform 1 Pose: [-21.2260334370419;5.54461337984139;-0.0155332958557766]
Robot Arm 1 end: [5.61139000223918;-0.204141995828692;1.40014994639476]
Robot Arm 1 togoal: 11.1085109856784
Mobile Platform 2 Pose: [18.632062788413;-6.78705049881276;-4.45652910560599]
Robot Arm 2 end: [-6.75197320264829;-4.63596801683859;0.618091115093971]
Robot Arm 1 togoal: 6.33543697365348

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','1.020280e+00','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 394:
Mobile Platform 1 Pose: [-21.1773883102605;5.51465970676431;0.00306600344554369]
Robot Arm 1 end: [5.66388168371692;-0.306591114910131;1.35822400221404]
Robot Arm 1 togoal: 11.1827652433647
Mobile Platform 2 Pose: [18.8830752473865;-6.98287862019537;-4.4667566595957]
Robot Arm 2 end: [-6.8577222298142;-4.23670010318969;0.644483257795666]
Robot Arm 1 togoal: 6.62252345295523

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.736736e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 395:
Mobile Platform 1 Pose: [-21.0947475033197;5.4564039708364;0.0148067894175209]
Robot Arm 1 end: [5.81590376567688;-0.387648353955743;1.22980143835147]
Robot Arm 1 togoal: 11.2024415296766
Mobile Platform 2 Pose: [19.1082561128399;-7.15589274872648;-4.57356467032156]
Robot Arm 2 end: [-7.17411029871181;-4.68063342732976;0.699202394632561]
Robot Arm 1 togoal: 6.07919934222228

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.946949e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.150573e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 396:
Mobile Platform 1 Pose: [-21.0962037315003;5.44365680311756;0.00740975978634516]
Robot Arm 1 end: [5.89657977458196;-0.365652895364439;1.17853549314117]
Robot Arm 1 togoal: 11.1535689100846
Mobile Platform 2 Pose: [18.7411787903385;-6.89310864401118;-4.53967449152001]
Robot Arm 2 end: [-6.50864444202677;-4.33681491591755;0.762474441682823]
Robot Arm 1 togoal: 6.69601298918878

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','8.213809e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.330212e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 397:
Mobile Platform 1 Pose: [-21.2075876727262;5.48952566790239;-0.0161488121601927]
Robot Arm 1 end: [6.03014301539268;-0.260089855457589;1.10698963801221]
Robot Arm 1 togoal: 11.0091149916845
Mobile Platform 2 Pose: [18.1554371800797;-6.44089330409069;-4.47046408146533]
Robot Arm 2 end: [-5.96210252102253;-4.24303410204283;0.722862475573766]
Robot Arm 1 togoal: 7.07702592258017

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.903417e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','2.800823e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 398:
Mobile Platform 1 Pose: [-21.7020171506543;5.75512257690209;-0.0748478739102271]
Robot Arm 1 end: [6.02561638114256;-0.218012102692177;1.16336327077715]
Robot Arm 1 togoal: 10.9695670156436
Mobile Platform 2 Pose: [17.7947896277617;-6.19148669727256;-4.45117920989016]
Robot Arm 2 end: [-5.71069325435576;-4.36908418661173;0.557302715072253]
Robot Arm 1 togoal: 7.14381394723241

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','4.378921e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','3.000000e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
Step 399:
Mobile Platform 1 Pose: [-21.8571933615214;5.87543232238631;-0.0551758378997787]
Robot Arm 1 end: [5.42686443562544;-0.292422767732733;1.19376946330516]
Robot Arm 1 togoal: 11.2674138622861
Mobile Platform 2 Pose: [17.2554873552797;-5.83609478267422;-4.48101923371094]
Robot Arm 2 end: [-5.37648113593007;-4.78944511687296;-0.0446658798472835]
Robot Arm 1 togoal: 7.13989625441488

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','7.778399e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>

<a href = "matlab: helpview('optim','no_feasible_point','CSHelpWindow');">Converged to an infeasible point</a>.

fmincon stopped because the <a href = "matlab: helpview('optim','norm_curr_step_simple_fminconip','CSHelpWindow');">size of the current step</a> is less than
the value of the <a href = "matlab: helpview('optim','step_size_tol','CSHelpWindow');">step size tolerance</a> but constraints are not
satisfied to within the value of the <a href = "matlab: helpview('optim','constraint_tolerance','CSHelpWindow');">constraint tolerance</a>.

<<a href = "matlab: createExitMsg({'optimlib:sqpLineSearch:ExitNeg22basic','fmincon'},{'optimlib:sqpLineSearch:ExitNeg22detailed','1.000000e-06','9.459992e-01','1.000000e-06'},true,true);;">stopping criteria details</a>>
